
<!DOCTYPE html>

<html xmlns="http://www.w3.org/1999/xhtml">
  <head>
    <meta charset="utf-8" />
    <title>spatialmath.base.transforms3d &#8212; Spatial Maths package 0.7.0
 documentation</title>
    <link rel="stylesheet" href="../../../_static/alabaster.css" type="text/css" />
    <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
    <link rel="stylesheet" type="text/css" href="../../../_static/graphviz.css" />
    <script id="documentation_options" data-url_root="../../../" src="../../../_static/documentation_options.js"></script>
    <script src="../../../_static/jquery.js"></script>
    <script src="../../../_static/underscore.js"></script>
    <script src="../../../_static/doctools.js"></script>
    <script src="../../../_static/language_data.js"></script>
    <script async="async" src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.5/latest.js?config=TeX-AMS-MML_HTMLorMML"></script>
    <link rel="index" title="Index" href="../../../genindex.html" />
    <link rel="search" title="Search" href="../../../search.html" />
   
  <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
  
  
  <meta name="viewport" content="width=device-width, initial-scale=0.9, maximum-scale=0.9" />

  </head><body>
  

    <div class="document">
      <div class="documentwrapper">
        <div class="bodywrapper">
          

          <div class="body" role="main">
            
  <h1>Source code for spatialmath.base.transforms3d</h1><div class="highlight"><pre>
<span></span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">This modules contains functions to create and transform 3D rotation matrices</span>
<span class="sd">and homogeneous tranformation matrices.</span>

<span class="sd">Vector arguments are what numpy refers to as ``array_like`` and can be a list,</span>
<span class="sd">tuple, numpy array, numpy row vector or numpy column vector.</span>

<span class="sd">TODO:</span>

<span class="sd">    - trinterp</span>
<span class="sd">    - trjac, trjac2</span>
<span class="sd">    - tranimate, tranimate2</span>
<span class="sd">&quot;&quot;&quot;</span>

<span class="c1"># This file is part of the SpatialMath toolbox for Python</span>
<span class="c1"># https://github.com/petercorke/spatialmath-python</span>
<span class="c1"># </span>
<span class="c1"># MIT License</span>
<span class="c1"># </span>
<span class="c1"># Copyright (c) 1993-2020 Peter Corke</span>
<span class="c1"># </span>
<span class="c1"># Permission is hereby granted, free of charge, to any person obtaining a copy</span>
<span class="c1"># of this software and associated documentation files (the &quot;Software&quot;), to deal</span>
<span class="c1"># in the Software without restriction, including without limitation the rights</span>
<span class="c1"># to use, copy, modify, merge, publish, distribute, sublicense, and/or sell</span>
<span class="c1"># copies of the Software, and to permit persons to whom the Software is</span>
<span class="c1"># furnished to do so, subject to the following conditions:</span>
<span class="c1"># </span>
<span class="c1"># The above copyright notice and this permission notice shall be included in all</span>
<span class="c1"># copies or substantial portions of the Software.</span>
<span class="c1"># </span>
<span class="c1"># THE SOFTWARE IS PROVIDED &quot;AS IS&quot;, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR</span>
<span class="c1"># IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,</span>
<span class="c1"># FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE</span>
<span class="c1"># AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER</span>
<span class="c1"># LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,</span>
<span class="c1"># OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE</span>
<span class="c1"># SOFTWARE.</span>

<span class="c1"># Contributors:</span>
<span class="c1"># </span>
<span class="c1">#     1. Luis Fernando Lara Tobar and Peter Corke, 2008</span>
<span class="c1">#     2. Josh Carrigg Hodson, Aditya Dua, Chee Ho Chan, 2017 (robopy)</span>
<span class="c1">#     3. Peter Corke, 2020</span>


<span class="kn">import</span> <span class="nn">sys</span>
<span class="kn">import</span> <span class="nn">math</span>
<span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
<span class="kn">from</span> <span class="nn">spatialmath.base</span> <span class="kn">import</span> <span class="n">argcheck</span>
<span class="kn">from</span> <span class="nn">spatialmath.base</span> <span class="kn">import</span> <span class="n">vectors</span> <span class="k">as</span> <span class="n">vec</span>
<span class="kn">from</span> <span class="nn">spatialmath.base</span> <span class="kn">import</span> <span class="n">transformsNd</span> <span class="k">as</span> <span class="n">trn</span>
<span class="kn">from</span> <span class="nn">spatialmath.base</span> <span class="kn">import</span> <span class="n">quaternions</span> <span class="k">as</span> <span class="n">quat</span>


<span class="k">try</span><span class="p">:</span>  <span class="c1"># pragma: no cover</span>
    <span class="c1"># print(&#39;Using SymPy&#39;)</span>
    <span class="kn">import</span> <span class="nn">sympy</span> <span class="k">as</span> <span class="nn">sym</span>

    <span class="k">def</span> <span class="nf">issymbol</span><span class="p">(</span><span class="n">x</span><span class="p">):</span>
        <span class="k">return</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">sym</span><span class="o">.</span><span class="n">Symbol</span><span class="p">)</span>
<span class="k">except</span> <span class="ne">BaseException</span><span class="p">:</span>
<div class="viewcode-block" id="issymbol"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.issymbol">[docs]</a>    <span class="k">def</span> <span class="nf">issymbol</span><span class="p">(</span><span class="n">x</span><span class="p">):</span>
        <span class="k">return</span> <span class="kc">False</span></div>

<span class="n">_eps</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">finfo</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">float64</span><span class="p">)</span><span class="o">.</span><span class="n">eps</span>


<div class="viewcode-block" id="colvec"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.colvec">[docs]</a><span class="k">def</span> <span class="nf">colvec</span><span class="p">(</span><span class="n">v</span><span class="p">):</span>
    <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">(</span><span class="n">v</span><span class="p">)</span><span class="o">.</span><span class="n">reshape</span><span class="p">((</span><span class="nb">len</span><span class="p">(</span><span class="n">v</span><span class="p">),</span> <span class="mi">1</span><span class="p">))</span></div>

<span class="c1"># ---------------------------------------------------------------------------------------#</span>


<span class="k">def</span> <span class="nf">_cos</span><span class="p">(</span><span class="n">theta</span><span class="p">):</span>
    <span class="k">if</span> <span class="n">issymbol</span><span class="p">(</span><span class="n">theta</span><span class="p">):</span>
        <span class="k">return</span> <span class="n">sym</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">return</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>


<span class="k">def</span> <span class="nf">_sin</span><span class="p">(</span><span class="n">theta</span><span class="p">):</span>
    <span class="k">if</span> <span class="n">issymbol</span><span class="p">(</span><span class="n">theta</span><span class="p">):</span>
        <span class="k">return</span> <span class="n">sym</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">return</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>


<div class="viewcode-block" id="rotx"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.rotx">[docs]</a><span class="k">def</span> <span class="nf">rotx</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s2">&quot;rad&quot;</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create SO(3) rotation about X-axis</span>

<span class="sd">    :param theta: rotation angle about X-axis</span>
<span class="sd">    :type theta: float</span>
<span class="sd">    :param unit: angular units: &#39;rad&#39; [default], or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :return: 3x3 rotation matrix</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(3,3)</span>

<span class="sd">    - ``rotx(THETA)`` is an SO(3) rotation matrix (3x3) representing a rotation</span>
<span class="sd">      of THETA radians about the x-axis</span>
<span class="sd">    - ``rotx(THETA, &quot;deg&quot;)`` as above but THETA is in degrees</span>

<span class="sd">    :seealso: :func:`~trotx`</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="n">theta</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getunit</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="p">)</span>
    <span class="n">ct</span> <span class="o">=</span> <span class="n">_cos</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>
    <span class="n">st</span> <span class="o">=</span> <span class="n">_sin</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>
    <span class="n">R</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span>
        <span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span>
        <span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="n">ct</span><span class="p">,</span> <span class="o">-</span><span class="n">st</span><span class="p">],</span>
        <span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="n">st</span><span class="p">,</span> <span class="n">ct</span><span class="p">]])</span>
    <span class="k">return</span> <span class="n">R</span></div>


<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="roty"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.roty">[docs]</a><span class="k">def</span> <span class="nf">roty</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s2">&quot;rad&quot;</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create SO(3) rotation about Y-axis</span>

<span class="sd">    :param theta: rotation angle about Y-axis</span>
<span class="sd">    :type theta: float</span>
<span class="sd">    :param unit: angular units: &#39;rad&#39; [default], or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :return: 3x3 rotation matrix</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(3,3)</span>

<span class="sd">    - ``roty(THETA)`` is an SO(3) rotation matrix (3x3) representing a rotation</span>
<span class="sd">      of THETA radians about the y-axis</span>
<span class="sd">    - ``roty(THETA, &quot;deg&quot;)`` as above but THETA is in degrees</span>

<span class="sd">    :seealso: :func:`~troty`</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="n">theta</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getunit</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="p">)</span>
    <span class="n">ct</span> <span class="o">=</span> <span class="n">_cos</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>
    <span class="n">st</span> <span class="o">=</span> <span class="n">_sin</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>
    <span class="n">R</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span>
        <span class="p">[</span><span class="n">ct</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="n">st</span><span class="p">],</span>
        <span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span>
        <span class="p">[</span><span class="o">-</span><span class="n">st</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="n">ct</span><span class="p">]])</span>
    <span class="k">return</span> <span class="n">R</span></div>


<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="rotz"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.rotz">[docs]</a><span class="k">def</span> <span class="nf">rotz</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s2">&quot;rad&quot;</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create SO(3) rotation about Z-axis</span>

<span class="sd">    :param theta: rotation angle about Z-axis</span>
<span class="sd">    :type theta: float</span>
<span class="sd">    :param unit: angular units: &#39;rad&#39; [default], or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :return: 3x3 rotation matrix</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(3,3)</span>

<span class="sd">    - ``rotz(THETA)`` is an SO(3) rotation matrix (3x3) representing a rotation</span>
<span class="sd">      of THETA radians about the z-axis</span>
<span class="sd">    - ``rotz(THETA, &quot;deg&quot;)`` as above but THETA is in degrees</span>

<span class="sd">    :seealso: :func:`~yrotz`</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">theta</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getunit</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="p">)</span>
    <span class="n">ct</span> <span class="o">=</span> <span class="n">_cos</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>
    <span class="n">st</span> <span class="o">=</span> <span class="n">_sin</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>
    <span class="n">R</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span>
        <span class="p">[</span><span class="n">ct</span><span class="p">,</span> <span class="o">-</span><span class="n">st</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span>
        <span class="p">[</span><span class="n">st</span><span class="p">,</span> <span class="n">ct</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span>
        <span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">]])</span>
    <span class="k">return</span> <span class="n">R</span></div>


<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="trotx"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.trotx">[docs]</a><span class="k">def</span> <span class="nf">trotx</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s2">&quot;rad&quot;</span><span class="p">,</span> <span class="n">t</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create SE(3) pure rotation about X-axis</span>

<span class="sd">    :param theta: rotation angle about X-axis</span>
<span class="sd">    :type theta: float</span>
<span class="sd">    :param unit: angular units: &#39;rad&#39; [default], or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :param t: translation 3-vector, defaults to [0,0,0]</span>
<span class="sd">    :type t: array_like    :return: 4x4 homogeneous transformation matrix</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(4,4)</span>

<span class="sd">    - ``trotx(THETA)`` is a homogeneous transformation (4x4) representing a rotation</span>
<span class="sd">      of THETA radians about the x-axis.</span>
<span class="sd">    - ``trotx(THETA, &#39;deg&#39;)`` as above but THETA is in degrees</span>
<span class="sd">    - ``trotx(THETA, &#39;rad&#39;, t=[x,y,z])`` as above with translation of [x,y,z]</span>

<span class="sd">    :seealso: :func:`~rotx`</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">T</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">pad</span><span class="p">(</span><span class="n">rotx</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="p">),</span> <span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">),</span> <span class="n">mode</span><span class="o">=</span><span class="s1">&#39;constant&#39;</span><span class="p">)</span>
    <span class="k">if</span> <span class="n">t</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span><span class="p">:</span>
        <span class="n">T</span><span class="p">[:</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="mi">3</span><span class="p">,</span> <span class="s1">&#39;array&#39;</span><span class="p">)</span>
    <span class="n">T</span><span class="p">[</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="mf">1.0</span>
    <span class="k">return</span> <span class="n">T</span></div>


<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="troty"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.troty">[docs]</a><span class="k">def</span> <span class="nf">troty</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s2">&quot;rad&quot;</span><span class="p">,</span> <span class="n">t</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create SE(3) pure rotation about Y-axis</span>

<span class="sd">    :param theta: rotation angle about Y-axis</span>
<span class="sd">    :type theta: float</span>
<span class="sd">    :param unit: angular units: &#39;rad&#39; [default], or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :param t: translation 3-vector, defaults to [0,0,0]</span>
<span class="sd">    :type t: array_like</span>
<span class="sd">    :return: 4x4 homogeneous transformation matrix as a numpy array</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(4,4)</span>

<span class="sd">    - ``troty(THETA)`` is a homogeneous transformation (4x4) representing a rotation</span>
<span class="sd">      of THETA radians about the y-axis.</span>
<span class="sd">    - ``troty(THETA, &#39;deg&#39;)`` as above but THETA is in degrees</span>
<span class="sd">    - ``troty(THETA, &#39;rad&#39;, t=[x,y,z])`` as above with translation of [x,y,z]</span>

<span class="sd">    :seealso: :func:`~roty`</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">T</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">pad</span><span class="p">(</span><span class="n">roty</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="p">),</span> <span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">),</span> <span class="n">mode</span><span class="o">=</span><span class="s1">&#39;constant&#39;</span><span class="p">)</span>
    <span class="k">if</span> <span class="n">t</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span><span class="p">:</span>
        <span class="n">T</span><span class="p">[:</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="mi">3</span><span class="p">,</span> <span class="s1">&#39;array&#39;</span><span class="p">)</span>
    <span class="n">T</span><span class="p">[</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="mf">1.0</span>
    <span class="k">return</span> <span class="n">T</span></div>


<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="trotz"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.trotz">[docs]</a><span class="k">def</span> <span class="nf">trotz</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s2">&quot;rad&quot;</span><span class="p">,</span> <span class="n">t</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create SE(3) pure rotation about Z-axis</span>

<span class="sd">    :param theta: rotation angle about Z-axis</span>
<span class="sd">    :type theta: float</span>
<span class="sd">    :param unit: angular units: &#39;rad&#39; [default], or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :param t: translation 3-vector, defaults to [0,0,0]</span>
<span class="sd">    :type t: array_like</span>
<span class="sd">    :return: 4x4 homogeneous transformation matrix</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(4,4)</span>

<span class="sd">    - ``trotz(THETA)`` is a homogeneous transformation (4x4) representing a rotation</span>
<span class="sd">      of THETA radians about the z-axis.</span>
<span class="sd">    - ``trotz(THETA, &#39;deg&#39;)`` as above but THETA is in degrees</span>
<span class="sd">    - ``trotz(THETA, &#39;rad&#39;, t=[x,y,z])`` as above with translation of [x,y,z]</span>

<span class="sd">    :seealso: :func:`~rotz`</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">T</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">pad</span><span class="p">(</span><span class="n">rotz</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="p">),</span> <span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">),</span> <span class="n">mode</span><span class="o">=</span><span class="s1">&#39;constant&#39;</span><span class="p">)</span>
    <span class="k">if</span> <span class="n">t</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span><span class="p">:</span>
        <span class="n">T</span><span class="p">[:</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="mi">3</span><span class="p">,</span> <span class="s1">&#39;array&#39;</span><span class="p">)</span>
    <span class="n">T</span><span class="p">[</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="mf">1.0</span>
    <span class="k">return</span> <span class="n">T</span></div>

<span class="c1"># ---------------------------------------------------------------------------------------#</span>


<div class="viewcode-block" id="transl"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.transl">[docs]</a><span class="k">def</span> <span class="nf">transl</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">z</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create SE(3) pure translation, or extract translation from SE(3) matrix</span>

<span class="sd">    :param x: translation along X-axis</span>
<span class="sd">    :type x: float</span>
<span class="sd">    :param y: translation along Y-axis</span>
<span class="sd">    :type y: float</span>
<span class="sd">    :param z: translation along Z-axis</span>
<span class="sd">    :type z: float</span>
<span class="sd">    :return: 4x4 homogeneous transformation matrix</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(4,4)</span>

<span class="sd">    Create a translational SE(3) matrix:</span>

<span class="sd">    - ``T = transl( X, Y, Z )`` is an SE(3) homogeneous transform (4x4) representing a</span>
<span class="sd">      pure translation of X, Y and Z.</span>
<span class="sd">    - ``T = transl( V )`` as above but the translation is given by a 3-element</span>
<span class="sd">      list, dict, or a numpy array, row or column vector.</span>


<span class="sd">    Extract the translational part of an SE(3) matrix:</span>

<span class="sd">    - ``P = TRANSL(T)`` is the translational part of a homogeneous transform T as a</span>
<span class="sd">      3-element numpy array.</span>

<span class="sd">    :seealso: :func:`~spatialmath.base.transforms2d.transl2`</span>
<span class="sd">   &quot;&quot;&quot;</span>

    <span class="k">if</span> <span class="n">np</span><span class="o">.</span><span class="n">isscalar</span><span class="p">(</span><span class="n">x</span><span class="p">):</span>
        <span class="n">T</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">identity</span><span class="p">(</span><span class="mi">4</span><span class="p">)</span>
        <span class="n">T</span><span class="p">[:</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="p">[</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">z</span><span class="p">]</span>
        <span class="k">return</span> <span class="n">T</span>
    <span class="k">elif</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">isvector</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="mi">3</span><span class="p">):</span>
        <span class="n">T</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">identity</span><span class="p">(</span><span class="mi">4</span><span class="p">)</span>
        <span class="n">T</span><span class="p">[:</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="mi">3</span><span class="p">,</span> <span class="n">out</span><span class="o">=</span><span class="s1">&#39;array&#39;</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">T</span>
    <span class="k">elif</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="p">(</span><span class="mi">4</span><span class="p">,</span> <span class="mi">4</span><span class="p">)):</span>
        <span class="k">return</span> <span class="n">x</span><span class="p">[:</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">]</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;bad argument&#39;</span><span class="p">)</span></div>


<div class="viewcode-block" id="ishom"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.ishom">[docs]</a><span class="k">def</span> <span class="nf">ishom</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">False</span><span class="p">,</span> <span class="n">tol</span><span class="o">=</span><span class="mi">10</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Test if matrix belongs to SE(3)</span>

<span class="sd">    :param T: matrix to test</span>
<span class="sd">    :type T: numpy.ndarray</span>
<span class="sd">    :param check: check validity of rotation submatrix</span>
<span class="sd">    :type check: bool</span>
<span class="sd">    :return: whether matrix is an SE(3) homogeneous transformation matrix</span>
<span class="sd">    :rtype: bool</span>

<span class="sd">    - ``ISHOM(T)`` is True if the argument ``T`` is of dimension 4x4</span>
<span class="sd">    - ``ISHOM(T, check=True)`` as above, but also checks orthogonality of the rotation sub-matrix and</span>
<span class="sd">      validitity of the bottom row.</span>

<span class="sd">    :seealso: :func:`~spatialmath.base.transformsNd.isR`, :func:`~isrot`, :func:`~spatialmath.base.transforms2d.ishom2`</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="k">return</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">)</span> <span class="ow">and</span> <span class="n">T</span><span class="o">.</span><span class="n">shape</span> <span class="o">==</span> <span class="p">(</span><span class="mi">4</span><span class="p">,</span> <span class="mi">4</span><span class="p">)</span> <span class="ow">and</span> <span class="p">(</span><span class="ow">not</span> <span class="n">check</span> <span class="ow">or</span> <span class="p">(</span><span class="n">trn</span><span class="o">.</span><span class="n">isR</span><span class="p">(</span><span class="n">T</span><span class="p">[:</span><span class="mi">3</span><span class="p">,</span> <span class="p">:</span><span class="mi">3</span><span class="p">],</span> <span class="n">tol</span><span class="o">=</span><span class="n">tol</span><span class="p">)</span> <span class="ow">and</span> <span class="n">np</span><span class="o">.</span><span class="n">all</span><span class="p">(</span><span class="n">T</span><span class="p">[</span><span class="mi">3</span><span class="p">,</span> <span class="p">:]</span> <span class="o">==</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">]))))</span></div>


<div class="viewcode-block" id="isrot"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.isrot">[docs]</a><span class="k">def</span> <span class="nf">isrot</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">False</span><span class="p">,</span> <span class="n">tol</span><span class="o">=</span><span class="mi">10</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Test if matrix belongs to SO(3)</span>

<span class="sd">    :param R: matrix to test</span>
<span class="sd">    :type R: numpy.ndarray</span>
<span class="sd">    :param check: check validity of rotation submatrix</span>
<span class="sd">    :type check: bool</span>
<span class="sd">    :return: whether matrix is an SO(3) rotation matrix</span>
<span class="sd">    :rtype: bool</span>

<span class="sd">    - ``ISROT(R)`` is True if the argument ``R`` is of dimension 3x3</span>
<span class="sd">    - ``ISROT(R, check=True)`` as above, but also checks orthogonality of the rotation matrix.</span>

<span class="sd">    :seealso: :func:`~spatialmath.base.transformsNd.isR`, :func:`~spatialmath.base.transforms2d.isrot2`,  :func:`~ishom`</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="k">return</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">)</span> <span class="ow">and</span> <span class="n">R</span><span class="o">.</span><span class="n">shape</span> <span class="o">==</span> <span class="p">(</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">)</span> <span class="ow">and</span> <span class="p">(</span><span class="ow">not</span> <span class="n">check</span> <span class="ow">or</span> <span class="n">trn</span><span class="o">.</span><span class="n">isR</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">tol</span><span class="o">=</span><span class="n">tol</span><span class="p">))</span></div>


<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="rpy2r"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.rpy2r">[docs]</a><span class="k">def</span> <span class="nf">rpy2r</span><span class="p">(</span><span class="n">roll</span><span class="p">,</span> <span class="n">pitch</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">yaw</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="o">*</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s1">&#39;rad&#39;</span><span class="p">,</span> <span class="n">order</span><span class="o">=</span><span class="s1">&#39;zyx&#39;</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create an SO(3) rotation matrix from roll-pitch-yaw angles</span>

<span class="sd">    :param roll: roll angle</span>
<span class="sd">    :type roll: float</span>
<span class="sd">    :param pitch: pitch angle</span>
<span class="sd">    :type pitch: float</span>
<span class="sd">    :param yaw: yaw angle</span>
<span class="sd">    :type yaw: float</span>
<span class="sd">    :param unit: angular units: &#39;rad&#39; [default], or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :param unit: rotation order: &#39;zyx&#39; [default], &#39;xyz&#39;, or &#39;yxz&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :return: 3x3 rotation matrix</span>
<span class="sd">    :rtype: numpdy.ndarray, shape=(3,3)</span>

<span class="sd">    - ``rpy2r(ROLL, PITCH, YAW)`` is an SO(3) orthonormal rotation matrix</span>
<span class="sd">      (3x3) equivalent to the specified roll, pitch, yaw angles angles.</span>
<span class="sd">      These correspond to successive rotations about the axes specified by ``order``:</span>

<span class="sd">        - &#39;zyx&#39; [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,</span>
<span class="sd">          then by roll about the new x-axis.  Convention for a mobile robot with x-axis forward</span>
<span class="sd">          and y-axis sideways.</span>
<span class="sd">        - &#39;xyz&#39;, rotate by yaw about the x-axis, then by pitch about the new y-axis,</span>
<span class="sd">          then by roll about the new z-axis. Covention for a robot gripper with z-axis forward</span>
<span class="sd">          and y-axis between the gripper fingers.</span>
<span class="sd">        - &#39;yxz&#39;, rotate by yaw about the y-axis, then by pitch about the new x-axis,</span>
<span class="sd">          then by roll about the new z-axis. Convention for a camera with z-axis parallel</span>
<span class="sd">          to the optic axis and x-axis parallel to the pixel rows.</span>

<span class="sd">    - ``rpy2r(RPY)`` as above but the roll, pitch, yaw angles are taken</span>
<span class="sd">      from ``RPY`` which is a 3-vector (array_like) with values</span>
<span class="sd">      (ROLL, PITCH, YAW).</span>

<span class="sd">    :seealso: :func:`~eul2r`, :func:`~rpy2tr`, :func:`~tr2rpy`</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="k">if</span> <span class="n">np</span><span class="o">.</span><span class="n">isscalar</span><span class="p">(</span><span class="n">roll</span><span class="p">):</span>
        <span class="n">angles</span> <span class="o">=</span> <span class="p">[</span><span class="n">roll</span><span class="p">,</span> <span class="n">pitch</span><span class="p">,</span> <span class="n">yaw</span><span class="p">]</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="n">angles</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">roll</span><span class="p">,</span> <span class="mi">3</span><span class="p">)</span>

    <span class="n">angles</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getunit</span><span class="p">(</span><span class="n">angles</span><span class="p">,</span> <span class="n">unit</span><span class="p">)</span>

    <span class="k">if</span> <span class="n">order</span> <span class="o">==</span> <span class="s1">&#39;xyz&#39;</span> <span class="ow">or</span> <span class="n">order</span> <span class="o">==</span> <span class="s1">&#39;arm&#39;</span><span class="p">:</span>
        <span class="n">R</span> <span class="o">=</span> <span class="n">rotx</span><span class="p">(</span><span class="n">angles</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span> <span class="o">@</span> <span class="n">roty</span><span class="p">(</span><span class="n">angles</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span> <span class="o">@</span> <span class="n">rotz</span><span class="p">(</span><span class="n">angles</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span>
    <span class="k">elif</span> <span class="n">order</span> <span class="o">==</span> <span class="s1">&#39;zyx&#39;</span> <span class="ow">or</span> <span class="n">order</span> <span class="o">==</span> <span class="s1">&#39;vehicle&#39;</span><span class="p">:</span>
        <span class="n">R</span> <span class="o">=</span> <span class="n">rotz</span><span class="p">(</span><span class="n">angles</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span> <span class="o">@</span> <span class="n">roty</span><span class="p">(</span><span class="n">angles</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span> <span class="o">@</span> <span class="n">rotx</span><span class="p">(</span><span class="n">angles</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span>
    <span class="k">elif</span> <span class="n">order</span> <span class="o">==</span> <span class="s1">&#39;yxz&#39;</span> <span class="ow">or</span> <span class="n">order</span> <span class="o">==</span> <span class="s1">&#39;camera&#39;</span><span class="p">:</span>
        <span class="n">R</span> <span class="o">=</span> <span class="n">roty</span><span class="p">(</span><span class="n">angles</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span> <span class="o">@</span> <span class="n">rotx</span><span class="p">(</span><span class="n">angles</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span> <span class="o">@</span> <span class="n">rotz</span><span class="p">(</span><span class="n">angles</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;Invalid angle order&#39;</span><span class="p">)</span>

    <span class="k">return</span> <span class="n">R</span></div>


<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="rpy2tr"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.rpy2tr">[docs]</a><span class="k">def</span> <span class="nf">rpy2tr</span><span class="p">(</span><span class="n">roll</span><span class="p">,</span> <span class="n">pitch</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">yaw</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s1">&#39;rad&#39;</span><span class="p">,</span> <span class="n">order</span><span class="o">=</span><span class="s1">&#39;zyx&#39;</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create an SE(3) rotation matrix from roll-pitch-yaw angles</span>

<span class="sd">    :param roll: roll angle</span>
<span class="sd">    :type roll: float</span>
<span class="sd">    :param pitch: pitch angle</span>
<span class="sd">    :type pitch: float</span>
<span class="sd">    :param yaw: yaw angle</span>
<span class="sd">    :type yaw: float</span>
<span class="sd">    :param unit: angular units: &#39;rad&#39; [default], or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :param unit: rotation order: &#39;zyx&#39; [default], &#39;xyz&#39;, or &#39;yxz&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :return: 3x3 rotation matrix</span>
<span class="sd">    :rtype: numpdy.ndarray, shape=(3,3)</span>

<span class="sd">    - ``rpy2tr(ROLL, PITCH, YAW)`` is an SO(3) orthonormal rotation matrix</span>
<span class="sd">      (3x3) equivalent to the specified roll, pitch, yaw angles angles.</span>
<span class="sd">      These correspond to successive rotations about the axes specified by ``order``:</span>

<span class="sd">        - &#39;zyx&#39; [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,</span>
<span class="sd">          then by roll about the new x-axis.  Convention for a mobile robot with x-axis forward</span>
<span class="sd">          and y-axis sideways.</span>
<span class="sd">        - &#39;xyz&#39;, rotate by yaw about the x-axis, then by pitch about the new y-axis,</span>
<span class="sd">          then by roll about the new z-axis. Convention for a robot gripper with z-axis forward</span>
<span class="sd">          and y-axis between the gripper fingers.</span>
<span class="sd">        - &#39;yxz&#39;, rotate by yaw about the y-axis, then by pitch about the new x-axis,</span>
<span class="sd">          then by roll about the new z-axis. Convention for a camera with z-axis parallel</span>
<span class="sd">          to the optic axis and x-axis parallel to the pixel rows.</span>

<span class="sd">    - ``rpy2tr(RPY)`` as above but the roll, pitch, yaw angles are taken</span>
<span class="sd">      from ``RPY`` which is a 3-vector (array_like) with values</span>
<span class="sd">      (ROLL, PITCH, YAW).</span>

<span class="sd">    Notes:</span>

<span class="sd">    - The translational part is zero.</span>

<span class="sd">    :seealso: :func:`~eul2tr`, :func:`~rpy2r`, :func:`~tr2rpy`</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="n">R</span> <span class="o">=</span> <span class="n">rpy2r</span><span class="p">(</span><span class="n">roll</span><span class="p">,</span> <span class="n">pitch</span><span class="p">,</span> <span class="n">yaw</span><span class="p">,</span> <span class="n">order</span><span class="o">=</span><span class="n">order</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="n">unit</span><span class="p">)</span>
    <span class="k">return</span> <span class="n">trn</span><span class="o">.</span><span class="n">r2t</span><span class="p">(</span><span class="n">R</span><span class="p">)</span></div>

<span class="c1"># ---------------------------------------------------------------------------------------#</span>


<div class="viewcode-block" id="eul2r"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.eul2r">[docs]</a><span class="k">def</span> <span class="nf">eul2r</span><span class="p">(</span><span class="n">phi</span><span class="p">,</span> <span class="n">theta</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">psi</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s1">&#39;rad&#39;</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create an SO(3) rotation matrix from Euler angles</span>

<span class="sd">    :param phi: Z-axis rotation</span>
<span class="sd">    :type phi: float</span>
<span class="sd">    :param theta: Y-axis rotation</span>
<span class="sd">    :type theta: float</span>
<span class="sd">    :param psi: Z-axis rotation</span>
<span class="sd">    :type psi: float</span>
<span class="sd">    :param unit: angular units: &#39;rad&#39; [default], or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :return: 3x3 rotation matrix</span>
<span class="sd">    :rtype: numpdy.ndarray, shape=(3,3)</span>

<span class="sd">    - ``R = eul2r(PHI, THETA, PSI)`` is an SO(3) orthonornal rotation</span>
<span class="sd">      matrix equivalent to the specified Euler angles.  These correspond</span>
<span class="sd">      to rotations about the Z, Y, Z axes respectively.</span>
<span class="sd">    - ``R = eul2r(EUL)`` as above but the Euler angles are taken from</span>
<span class="sd">      ``EUL`` which is a 3-vector (array_like) with values</span>
<span class="sd">      (PHI THETA PSI).</span>

<span class="sd">    :seealso: :func:`~rpy2r`, :func:`~eul2tr`, :func:`~tr2eul`</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="k">if</span> <span class="n">np</span><span class="o">.</span><span class="n">isscalar</span><span class="p">(</span><span class="n">phi</span><span class="p">):</span>
        <span class="n">angles</span> <span class="o">=</span> <span class="p">[</span><span class="n">phi</span><span class="p">,</span> <span class="n">theta</span><span class="p">,</span> <span class="n">psi</span><span class="p">]</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="n">angles</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">phi</span><span class="p">,</span> <span class="mi">3</span><span class="p">)</span>

    <span class="n">angles</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getunit</span><span class="p">(</span><span class="n">angles</span><span class="p">,</span> <span class="n">unit</span><span class="p">)</span>

    <span class="k">return</span> <span class="n">rotz</span><span class="p">(</span><span class="n">angles</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span> <span class="o">@</span> <span class="n">roty</span><span class="p">(</span><span class="n">angles</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span> <span class="o">@</span> <span class="n">rotz</span><span class="p">(</span><span class="n">angles</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span></div>


<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="eul2tr"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.eul2tr">[docs]</a><span class="k">def</span> <span class="nf">eul2tr</span><span class="p">(</span><span class="n">phi</span><span class="p">,</span> <span class="n">theta</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">psi</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s1">&#39;rad&#39;</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create an SE(3) pure rotation matrix from Euler angles</span>

<span class="sd">    :param phi: Z-axis rotation</span>
<span class="sd">    :type phi: float</span>
<span class="sd">    :param theta: Y-axis rotation</span>
<span class="sd">    :type theta: float</span>
<span class="sd">    :param psi: Z-axis rotation</span>
<span class="sd">    :type psi: float</span>
<span class="sd">    :param unit: angular units: &#39;rad&#39; [default], or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :return: 4x4 homogeneous transformation matrix</span>
<span class="sd">    :rtype: numpdy.ndarray, shape=(4,4)</span>

<span class="sd">    - ``R = eul2tr(PHI, THETA, PSI)`` is an SE(3) homogeneous transformation</span>
<span class="sd">      matrix equivalent to the specified Euler angles.  These correspond</span>
<span class="sd">      to rotations about the Z, Y, Z axes respectively.</span>
<span class="sd">    - ``R = eul2tr(EUL)`` as above but the Euler angles are taken from</span>
<span class="sd">      ``EUL`` which is a 3-vector (array_like) with values</span>
<span class="sd">      (PHI THETA PSI).</span>

<span class="sd">    Notes:</span>

<span class="sd">    - The translational part is zero.</span>

<span class="sd">    :seealso: :func:`~rpy2tr`, :func:`~eul2r`, :func:`~tr2eul`</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="n">R</span> <span class="o">=</span> <span class="n">eul2r</span><span class="p">(</span><span class="n">phi</span><span class="p">,</span> <span class="n">theta</span><span class="p">,</span> <span class="n">psi</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="n">unit</span><span class="p">)</span>
    <span class="k">return</span> <span class="n">trn</span><span class="o">.</span><span class="n">r2t</span><span class="p">(</span><span class="n">R</span><span class="p">)</span></div>

<span class="c1"># ---------------------------------------------------------------------------------------#</span>


<div class="viewcode-block" id="angvec2r"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.angvec2r">[docs]</a><span class="k">def</span> <span class="nf">angvec2r</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">v</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s1">&#39;rad&#39;</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create an SO(3) rotation matrix from rotation angle and axis</span>

<span class="sd">    :param theta: rotation</span>
<span class="sd">    :type theta: float</span>
<span class="sd">    :param unit: angular units: &#39;rad&#39; [default], or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :param v: rotation axis, 3-vector</span>
<span class="sd">    :type v: array_like</span>
<span class="sd">    :return: 3x3 rotation matrix</span>
<span class="sd">    :rtype: numpdy.ndarray, shape=(3,3)</span>

<span class="sd">    ``angvec2r(THETA, V)`` is an SO(3) orthonormal rotation matrix</span>
<span class="sd">    equivalent to a rotation of ``THETA`` about the vector ``V``.</span>

<span class="sd">    Notes:</span>

<span class="sd">    - If ``THETA == 0`` then return identity matrix.</span>
<span class="sd">    - If ``THETA ~= 0`` then ``V`` must have a finite length.</span>

<span class="sd">    :seealso: :func:`~angvec2tr`, :func:`~tr2angvec`</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="k">assert</span> <span class="n">np</span><span class="o">.</span><span class="n">isscalar</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span> <span class="ow">and</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">isvector</span><span class="p">(</span><span class="n">v</span><span class="p">,</span> <span class="mi">3</span><span class="p">),</span> <span class="s2">&quot;Arguments must be theta and vector&quot;</span>

    <span class="k">if</span> <span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">v</span><span class="p">)</span> <span class="o">&lt;</span> <span class="mi">10</span> <span class="o">*</span> <span class="n">_eps</span><span class="p">:</span>
        <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span>

    <span class="n">theta</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getunit</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="p">)</span>

    <span class="c1"># Rodrigue&#39;s equation</span>

    <span class="n">sk</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">skew</span><span class="p">(</span><span class="n">vec</span><span class="o">.</span><span class="n">unitvec</span><span class="p">(</span><span class="n">v</span><span class="p">))</span>
    <span class="n">R</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span> <span class="o">+</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span> <span class="o">*</span> <span class="n">sk</span> <span class="o">+</span> <span class="p">(</span><span class="mf">1.0</span> <span class="o">-</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">theta</span><span class="p">))</span> <span class="o">*</span> <span class="n">sk</span> <span class="o">@</span> <span class="n">sk</span>
    <span class="k">return</span> <span class="n">R</span></div>


<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="angvec2tr"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.angvec2tr">[docs]</a><span class="k">def</span> <span class="nf">angvec2tr</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">v</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s1">&#39;rad&#39;</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create an SE(3) pure rotation from rotation angle and axis</span>

<span class="sd">    :param theta: rotation</span>
<span class="sd">    :type theta: float</span>
<span class="sd">    :param unit: angular units: &#39;rad&#39; [default], or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :param v: rotation axis, 3-vector</span>
<span class="sd">    :type v: : array_like</span>
<span class="sd">    :return: 4x4 homogeneous transformation matrix</span>
<span class="sd">    :rtype: numpdy.ndarray, shape=(4,4)</span>

<span class="sd">    ``angvec2tr(THETA, V)`` is an SE(3) homogeneous transformation matrix</span>
<span class="sd">    equivalent to a rotation of ``THETA`` about the vector ``V``.</span>

<span class="sd">    Notes:</span>

<span class="sd">    - If ``THETA == 0`` then return identity matrix.</span>
<span class="sd">    - If ``THETA ~= 0`` then ``V`` must have a finite length.</span>
<span class="sd">    - The translational part is zero.</span>

<span class="sd">    :seealso: :func:`~angvec2r`, :func:`~tr2angvec`</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="k">return</span> <span class="n">trn</span><span class="o">.</span><span class="n">r2t</span><span class="p">(</span><span class="n">angvec2r</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">v</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="n">unit</span><span class="p">))</span></div>


<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="oa2r"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.oa2r">[docs]</a><span class="k">def</span> <span class="nf">oa2r</span><span class="p">(</span><span class="n">o</span><span class="p">,</span> <span class="n">a</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create SO(3) rotation matrix from two vectors</span>

<span class="sd">    :param o: 3-vector parallel to Y- axis</span>
<span class="sd">    :type o: array_like</span>
<span class="sd">    :param a: 3-vector parallel to the Z-axis</span>
<span class="sd">    :type o: array_like</span>
<span class="sd">    :return: 3x3 rotation matrix</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(3,3)</span>

<span class="sd">    ``T = oa2tr(O, A)`` is an SO(3) orthonormal rotation matrix for a frame defined in terms of</span>
<span class="sd">    vectors parallel to its Y- and Z-axes with respect to a reference frame.  In robotics these axes are</span>
<span class="sd">    respectively called the orientation and approach vectors defined such that</span>
<span class="sd">    R = [N O A] and N = O x A.</span>

<span class="sd">    Steps:</span>

<span class="sd">        1. N&#39; = O x A</span>
<span class="sd">        2. O&#39; = A x N</span>
<span class="sd">        3. normalize N&#39;, O&#39;, A</span>
<span class="sd">        4. stack horizontally into rotation matrix</span>

<span class="sd">    Notes:</span>

<span class="sd">    - The A vector is the only guaranteed to have the same direction in the resulting</span>
<span class="sd">      rotation matrix</span>
<span class="sd">    - O and A do not have to be unit-length, they are normalized</span>
<span class="sd">    - O and A do not have to be orthogonal, so long as they are not parallel</span>
<span class="sd">    - The vectors O and A are parallel to the Y- and Z-axes of the equivalent coordinate frame.</span>

<span class="sd">    :seealso: :func:`~oa2tr`</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">o</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">o</span><span class="p">,</span> <span class="mi">3</span><span class="p">,</span> <span class="n">out</span><span class="o">=</span><span class="s1">&#39;array&#39;</span><span class="p">)</span>
    <span class="n">a</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">a</span><span class="p">,</span> <span class="mi">3</span><span class="p">,</span> <span class="n">out</span><span class="o">=</span><span class="s1">&#39;array&#39;</span><span class="p">)</span>
    <span class="n">n</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">o</span><span class="p">,</span> <span class="n">a</span><span class="p">)</span>
    <span class="n">o</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">a</span><span class="p">,</span> <span class="n">n</span><span class="p">)</span>
    <span class="n">R</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">stack</span><span class="p">((</span><span class="n">vec</span><span class="o">.</span><span class="n">unitvec</span><span class="p">(</span><span class="n">n</span><span class="p">),</span> <span class="n">vec</span><span class="o">.</span><span class="n">unitvec</span><span class="p">(</span><span class="n">o</span><span class="p">),</span> <span class="n">vec</span><span class="o">.</span><span class="n">unitvec</span><span class="p">(</span><span class="n">a</span><span class="p">)),</span> <span class="n">axis</span><span class="o">=</span><span class="mi">1</span><span class="p">)</span>
    <span class="k">return</span> <span class="n">R</span></div>


<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="oa2tr"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.oa2tr">[docs]</a><span class="k">def</span> <span class="nf">oa2tr</span><span class="p">(</span><span class="n">o</span><span class="p">,</span> <span class="n">a</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create SE(3) pure rotation from two vectors</span>

<span class="sd">    :param o: 3-vector parallel to Y- axis</span>
<span class="sd">    :type o: array_like</span>
<span class="sd">    :param a: 3-vector parallel to the Z-axis</span>
<span class="sd">    :type o: array_like</span>
<span class="sd">    :return: 4x4 homogeneous transformation matrix</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(4,4)</span>

<span class="sd">    ``T = oa2tr(O, A)`` is an SE(3) homogeneous transformation matrix for a frame defined in terms of</span>
<span class="sd">    vectors parallel to its Y- and Z-axes with respect to a reference frame.  In robotics these axes are</span>
<span class="sd">    respectively called the orientation and approach vectors defined such that</span>
<span class="sd">    R = [N O A] and N = O x A.</span>

<span class="sd">    Steps:</span>

<span class="sd">        1. N&#39; = O x A</span>
<span class="sd">        2. O&#39; = A x N</span>
<span class="sd">        3. normalize N&#39;, O&#39;, A</span>
<span class="sd">        4. stack horizontally into rotation matrix</span>

<span class="sd">    Notes:</span>

<span class="sd">    - The A vector is the only guaranteed to have the same direction in the resulting</span>
<span class="sd">      rotation matrix</span>
<span class="sd">    - O and A do not have to be unit-length, they are normalized</span>
<span class="sd">    - O and A do not have to be orthogonal, so long as they are not parallel</span>
<span class="sd">    - The translational part is zero.</span>
<span class="sd">    - The vectors O and A are parallel to the Y- and Z-axes of the equivalent coordinate frame.</span>

<span class="sd">    :seealso: :func:`~oa2r`</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="k">return</span> <span class="n">trn</span><span class="o">.</span><span class="n">r2t</span><span class="p">(</span><span class="n">oa2r</span><span class="p">(</span><span class="n">o</span><span class="p">,</span> <span class="n">a</span><span class="p">))</span></div>


<span class="c1"># ------------------------------------------------------------------------------------------------------------------- #</span>
<div class="viewcode-block" id="tr2angvec"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.tr2angvec">[docs]</a><span class="k">def</span> <span class="nf">tr2angvec</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s1">&#39;rad&#39;</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">False</span><span class="p">):</span>
    <span class="sa">r</span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Convert SO(3) or SE(3) to angle and rotation vector</span>

<span class="sd">    :param R: SO(3) or SE(3) matrix</span>
<span class="sd">    :type R: numpy.ndarray, shape=(3,3) or (4,4)</span>
<span class="sd">    :param unit: &#39;rad&#39; or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :param check: check that rotation matrix is valid</span>
<span class="sd">    :type check: bool</span>
<span class="sd">    :return: :math:`(\theta, {\bf v})`</span>
<span class="sd">    :rtype: float, numpy.ndarray, shape=(3,)</span>

<span class="sd">    ``tr2angvec(R)`` is a rotation angle and a vector about which the rotation</span>
<span class="sd">    acts that corresponds to the rotation part of ``R``.</span>

<span class="sd">    By default the angle is in radians but can be changed setting `unit=&#39;deg&#39;`.</span>

<span class="sd">    Notes:</span>

<span class="sd">    - If the input is SE(3) the translation component is ignored.</span>

<span class="sd">    :seealso: :func:`~angvec2r`, :func:`~angvec2tr`, :func:`~tr2rpy`, :func:`~tr2eul`</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="k">if</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="p">(</span><span class="mi">4</span><span class="p">,</span> <span class="mi">4</span><span class="p">)):</span>
        <span class="n">R</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">t2r</span><span class="p">(</span><span class="n">T</span><span class="p">)</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="n">R</span> <span class="o">=</span> <span class="n">T</span>
    <span class="k">assert</span> <span class="n">isrot</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="n">check</span><span class="p">)</span>

    <span class="n">v</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">vex</span><span class="p">(</span><span class="n">trlog</span><span class="p">(</span><span class="n">R</span><span class="p">))</span>

    <span class="k">if</span> <span class="n">vec</span><span class="o">.</span><span class="n">iszerovec</span><span class="p">(</span><span class="n">v</span><span class="p">):</span>
        <span class="n">theta</span> <span class="o">=</span> <span class="mi">0</span>
        <span class="n">v</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">r_</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="n">theta</span> <span class="o">=</span> <span class="n">vec</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">v</span><span class="p">)</span>
        <span class="n">v</span> <span class="o">=</span> <span class="n">vec</span><span class="o">.</span><span class="n">unitvec</span><span class="p">(</span><span class="n">v</span><span class="p">)</span>

    <span class="k">if</span> <span class="n">unit</span> <span class="o">==</span> <span class="s1">&#39;deg&#39;</span><span class="p">:</span>
        <span class="n">theta</span> <span class="o">*=</span> <span class="mi">180</span> <span class="o">/</span> <span class="n">math</span><span class="o">.</span><span class="n">pi</span>

    <span class="k">return</span> <span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">v</span><span class="p">)</span></div>


<span class="c1"># ------------------------------------------------------------------------------------------------------------------- #</span>
<div class="viewcode-block" id="tr2eul"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.tr2eul">[docs]</a><span class="k">def</span> <span class="nf">tr2eul</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s1">&#39;rad&#39;</span><span class="p">,</span> <span class="n">flip</span><span class="o">=</span><span class="kc">False</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">False</span><span class="p">):</span>
    <span class="sa">r</span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Convert SO(3) or SE(3) to ZYX Euler angles</span>

<span class="sd">    :param R: SO(3) or SE(3) matrix</span>
<span class="sd">    :type R: numpy.ndarray, shape=(3,3) or (4,4)</span>
<span class="sd">    :param unit: &#39;rad&#39; or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :param flip: choose first Euler angle to be in quadrant 2 or 3</span>
<span class="sd">    :type flip: bool</span>
<span class="sd">    :param check: check that rotation matrix is valid</span>
<span class="sd">    :type check: bool</span>
<span class="sd">    :return: ZYZ Euler angles</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(3,)</span>

<span class="sd">    ``tr2eul(R)`` are the Euler angles corresponding to</span>
<span class="sd">    the rotation part of ``R``.</span>

<span class="sd">    The 3 angles :math:`[\phi, \theta, \psi` correspond to sequential rotations about the</span>
<span class="sd">    Z, Y and Z axes respectively.</span>

<span class="sd">    By default the angles are in radians but can be changed setting `unit=&#39;deg&#39;`.</span>

<span class="sd">    Notes:</span>

<span class="sd">    - There is a singularity for the case where :math:`\theta=0` in which case :math:`\phi` is arbitrarily set to zero and :math:`\phi` is set to :math:`\phi+\psi`.</span>
<span class="sd">    - If the input is SE(3) the translation component is ignored.</span>

<span class="sd">    :seealso: :func:`~eul2r`, :func:`~eul2tr`, :func:`~tr2rpy`, :func:`~tr2angvec`</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="k">if</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="p">(</span><span class="mi">4</span><span class="p">,</span> <span class="mi">4</span><span class="p">)):</span>
        <span class="n">R</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">t2r</span><span class="p">(</span><span class="n">T</span><span class="p">)</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="n">R</span> <span class="o">=</span> <span class="n">T</span>
    <span class="k">assert</span> <span class="n">isrot</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="n">check</span><span class="p">)</span>

    <span class="n">eul</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">3</span><span class="p">,))</span>
    <span class="k">if</span> <span class="nb">abs</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span> <span class="o">&lt;</span> <span class="mi">10</span> <span class="o">*</span> <span class="n">_eps</span> <span class="ow">and</span> <span class="nb">abs</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span> <span class="o">&lt;</span> <span class="mi">10</span> <span class="o">*</span> <span class="n">_eps</span><span class="p">:</span>
        <span class="n">eul</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="mi">0</span>
        <span class="n">sp</span> <span class="o">=</span> <span class="mi">0</span>
        <span class="n">cp</span> <span class="o">=</span> <span class="mi">1</span>
        <span class="n">eul</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">cp</span> <span class="o">*</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">+</span> <span class="n">sp</span> <span class="o">*</span> <span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>
        <span class="n">eul</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="o">-</span><span class="n">sp</span> <span class="o">*</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">+</span> <span class="n">cp</span> <span class="o">*</span> <span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="o">-</span><span class="n">sp</span> <span class="o">*</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">+</span> <span class="n">cp</span> <span class="o">*</span> <span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">if</span> <span class="n">flip</span><span class="p">:</span>
            <span class="n">eul</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="o">-</span><span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">],</span> <span class="o">-</span><span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">eul</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>
        <span class="n">sp</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">eul</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span>
        <span class="n">cp</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">eul</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span>
        <span class="n">eul</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">cp</span> <span class="o">*</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">+</span> <span class="n">sp</span> <span class="o">*</span> <span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>
        <span class="n">eul</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="o">-</span><span class="n">sp</span> <span class="o">*</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">+</span> <span class="n">cp</span> <span class="o">*</span> <span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="o">-</span><span class="n">sp</span> <span class="o">*</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">+</span> <span class="n">cp</span> <span class="o">*</span> <span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>

    <span class="k">if</span> <span class="n">unit</span> <span class="o">==</span> <span class="s1">&#39;deg&#39;</span><span class="p">:</span>
        <span class="n">eul</span> <span class="o">*=</span> <span class="mi">180</span> <span class="o">/</span> <span class="n">math</span><span class="o">.</span><span class="n">pi</span>

    <span class="k">return</span> <span class="n">eul</span></div>

<span class="c1"># ------------------------------------------------------------------------------------------------------------------- #</span>


<div class="viewcode-block" id="tr2rpy"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.tr2rpy">[docs]</a><span class="k">def</span> <span class="nf">tr2rpy</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s1">&#39;rad&#39;</span><span class="p">,</span> <span class="n">order</span><span class="o">=</span><span class="s1">&#39;zyx&#39;</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">False</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Convert SO(3) or SE(3) to roll-pitch-yaw angles</span>

<span class="sd">    :param R: SO(3) or SE(3) matrix</span>
<span class="sd">    :type R: numpy.ndarray, shape=(3,3) or (4,4)</span>
<span class="sd">    :param unit: &#39;rad&#39; or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :param order: &#39;xyz&#39;, &#39;zyx&#39; or &#39;yxz&#39; [default &#39;zyx&#39;]</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :param check: check that rotation matrix is valid</span>
<span class="sd">    :type check: bool</span>
<span class="sd">    :return: Roll-pitch-yaw angles</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(3,)</span>

<span class="sd">    ``tr2rpy(R)`` are the roll-pitch-yaw angles corresponding to</span>
<span class="sd">    the rotation part of ``R``.</span>

<span class="sd">    The 3 angles RPY=[R,P,Y] correspond to sequential rotations about the</span>
<span class="sd">    Z, Y and X axes respectively.  The axis order sequence can be changed by</span>
<span class="sd">    setting:</span>

<span class="sd">    - `order=&#39;xyz&#39;`  for sequential rotations about X, Y, Z axes</span>
<span class="sd">    - `order=&#39;yxz&#39;`  for sequential rotations about Y, X, Z axes</span>

<span class="sd">    By default the angles are in radians but can be changed setting `unit=&#39;deg&#39;`.</span>

<span class="sd">    Notes:</span>

<span class="sd">    - There is a singularity for the case where P=:math:`\pi/2` in which case R is arbitrarily set to zero and Y is the sum (R+Y).</span>
<span class="sd">    - If the input is SE(3) the translation component is ignored.</span>

<span class="sd">    :seealso: :func:`~rpy2r`, :func:`~rpy2tr`, :func:`~tr2eul`, :func:`~tr2angvec`</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="k">if</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="p">(</span><span class="mi">4</span><span class="p">,</span> <span class="mi">4</span><span class="p">)):</span>
        <span class="n">R</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">t2r</span><span class="p">(</span><span class="n">T</span><span class="p">)</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="n">R</span> <span class="o">=</span> <span class="n">T</span>
    <span class="k">assert</span> <span class="n">isrot</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="n">check</span><span class="p">)</span>

    <span class="n">rpy</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">3</span><span class="p">,))</span>
    <span class="k">if</span> <span class="n">order</span> <span class="o">==</span> <span class="s1">&#39;xyz&#39;</span> <span class="ow">or</span> <span class="n">order</span> <span class="o">==</span> <span class="s1">&#39;arm&#39;</span><span class="p">:</span>

        <span class="c1"># XYZ order</span>
        <span class="k">if</span> <span class="nb">abs</span><span class="p">(</span><span class="nb">abs</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span> <span class="o">-</span> <span class="mi">1</span><span class="p">)</span> <span class="o">&lt;</span> <span class="mi">10</span> <span class="o">*</span> <span class="n">_eps</span><span class="p">:</span>  <span class="c1"># when |R13| == 1</span>
            <span class="c1"># singularity</span>
            <span class="n">rpy</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="mi">0</span>  <span class="c1"># roll is zero</span>
            <span class="k">if</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">&gt;</span> <span class="mi">0</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>   <span class="c1"># R+Y</span>
            <span class="k">else</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span>   <span class="c1"># R-Y</span>
            <span class="n">rpy</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">asin</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">rpy</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span>
            <span class="n">rpy</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>

            <span class="n">k</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">argmax</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">abs</span><span class="p">([</span><span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]]))</span>
            <span class="k">if</span> <span class="n">k</span> <span class="o">==</span> <span class="mi">0</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">rpy</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span> <span class="o">/</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span>
            <span class="k">elif</span> <span class="n">k</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">atan</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">rpy</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span> <span class="o">/</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>
            <span class="k">elif</span> <span class="n">k</span> <span class="o">==</span> <span class="mi">2</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">atan</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">rpy</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span> <span class="o">/</span> <span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>
            <span class="k">elif</span> <span class="n">k</span> <span class="o">==</span> <span class="mi">3</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">rpy</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span> <span class="o">/</span> <span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>

    <span class="k">elif</span> <span class="n">order</span> <span class="o">==</span> <span class="s1">&#39;zyx&#39;</span> <span class="ow">or</span> <span class="n">order</span> <span class="o">==</span> <span class="s1">&#39;vehicle&#39;</span><span class="p">:</span>

        <span class="c1"># old ZYX order (as per Paul book)</span>
        <span class="k">if</span> <span class="nb">abs</span><span class="p">(</span><span class="nb">abs</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span> <span class="o">-</span> <span class="mi">1</span><span class="p">)</span> <span class="o">&lt;</span> <span class="mi">10</span> <span class="o">*</span> <span class="n">_eps</span><span class="p">:</span>  <span class="c1"># when |R31| == 1</span>
            <span class="c1"># singularity</span>
            <span class="n">rpy</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="mi">0</span>     <span class="c1"># roll is zero</span>
            <span class="k">if</span> <span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">&lt;</span> <span class="mi">0</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>  <span class="c1"># R-Y</span>
            <span class="k">else</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="o">-</span><span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">],</span> <span class="o">-</span><span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>  <span class="c1"># R+Y</span>
            <span class="n">rpy</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">asin</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">rpy</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>  <span class="c1"># R</span>
            <span class="n">rpy</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span>  <span class="c1"># Y</span>

            <span class="n">k</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">argmax</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">abs</span><span class="p">([</span><span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]]))</span>
            <span class="k">if</span> <span class="n">k</span> <span class="o">==</span> <span class="mi">0</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">atan</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">rpy</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span> <span class="o">/</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span>
            <span class="k">elif</span> <span class="n">k</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">atan</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">rpy</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span> <span class="o">/</span> <span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span>
            <span class="k">elif</span> <span class="n">k</span> <span class="o">==</span> <span class="mi">2</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">atan</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">rpy</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span> <span class="o">/</span> <span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>
            <span class="k">elif</span> <span class="n">k</span> <span class="o">==</span> <span class="mi">3</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">atan</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">rpy</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span> <span class="o">/</span> <span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>

    <span class="k">elif</span> <span class="n">order</span> <span class="o">==</span> <span class="s1">&#39;yxz&#39;</span> <span class="ow">or</span> <span class="n">order</span> <span class="o">==</span> <span class="s1">&#39;camera&#39;</span><span class="p">:</span>

        <span class="k">if</span> <span class="nb">abs</span><span class="p">(</span><span class="nb">abs</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span> <span class="o">-</span> <span class="mi">1</span><span class="p">)</span> <span class="o">&lt;</span> <span class="mi">10</span> <span class="o">*</span> <span class="n">_eps</span><span class="p">:</span>  <span class="c1"># when |R23| == 1</span>
                <span class="c1"># singularity</span>
            <span class="n">rpy</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="mi">0</span>
            <span class="k">if</span> <span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">&lt;</span> <span class="mi">0</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span>   <span class="c1"># R-Y</span>
            <span class="k">else</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="o">-</span><span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="o">-</span><span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>   <span class="c1"># R+Y</span>
            <span class="n">rpy</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">asin</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>    <span class="c1"># P</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">rpy</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>
            <span class="n">rpy</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>

            <span class="n">k</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">argmax</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">abs</span><span class="p">([</span><span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">],</span> <span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]]))</span>
            <span class="k">if</span> <span class="n">k</span> <span class="o">==</span> <span class="mi">0</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">atan</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">rpy</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span> <span class="o">/</span> <span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span>
            <span class="k">elif</span> <span class="n">k</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">atan</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">rpy</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span> <span class="o">/</span> <span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>
            <span class="k">elif</span> <span class="n">k</span> <span class="o">==</span> <span class="mi">2</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">atan</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">rpy</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span> <span class="o">/</span> <span class="n">R</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>
            <span class="k">elif</span> <span class="n">k</span> <span class="o">==</span> <span class="mi">3</span><span class="p">:</span>
                <span class="n">rpy</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">math</span><span class="o">.</span><span class="n">atan</span><span class="p">(</span><span class="n">R</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">rpy</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span> <span class="o">/</span> <span class="n">R</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>

    <span class="k">else</span><span class="p">:</span>
        <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;Invalid order&#39;</span><span class="p">)</span>

    <span class="k">if</span> <span class="n">unit</span> <span class="o">==</span> <span class="s1">&#39;deg&#39;</span><span class="p">:</span>
        <span class="n">rpy</span> <span class="o">*=</span> <span class="mi">180</span> <span class="o">/</span> <span class="n">math</span><span class="o">.</span><span class="n">pi</span>

    <span class="k">return</span> <span class="n">rpy</span></div>


<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="trlog"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.trlog">[docs]</a><span class="k">def</span> <span class="nf">trlog</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">True</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Logarithm of SO(3) or SE(3) matrix</span>

<span class="sd">    :param T: SO(3) or SE(3) matrix</span>
<span class="sd">    :type T: numpy.ndarray, shape=(3,3) or (4,4)</span>
<span class="sd">    :return: logarithm</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(3,3) or (4,4)</span>
<span class="sd">    :raises: ValueError</span>

<span class="sd">    An efficient closed-form solution of the matrix logarithm for arguments that are SO(3) or SE(3).</span>

<span class="sd">    - ``trlog(R)`` is the logarithm of the passed rotation matrix ``R`` which will be</span>
<span class="sd">      3x3 skew-symmetric matrix.  The equivalent vector from ``vex()`` is parallel to rotation axis</span>
<span class="sd">      and its norm is the amount of rotation about that axis.</span>
<span class="sd">    - ``trlog(T)`` is the logarithm of the passed homogeneous transformation matrix ``T`` which will be</span>
<span class="sd">      4x4 augumented skew-symmetric matrix. The equivalent vector from ``vexa()`` is the twist</span>
<span class="sd">      vector (6x1) comprising [v w].</span>


<span class="sd">    :seealso: :func:`~trexp`, :func:`~spatialmath.base.transformsNd.vex`, :func:`~spatialmath.base.transformsNd.vexa`</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="k">if</span> <span class="n">ishom</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="n">check</span><span class="p">):</span>
        <span class="c1"># SE(3) matrix</span>

        <span class="k">if</span> <span class="n">trn</span><span class="o">.</span><span class="n">iseye</span><span class="p">(</span><span class="n">T</span><span class="p">):</span>
            <span class="c1"># is identity matrix</span>
            <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">4</span><span class="p">,</span> <span class="mi">4</span><span class="p">))</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="p">[</span><span class="n">R</span><span class="p">,</span> <span class="n">t</span><span class="p">]</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">tr2rt</span><span class="p">(</span><span class="n">T</span><span class="p">)</span>

            <span class="k">if</span> <span class="n">trn</span><span class="o">.</span><span class="n">iseye</span><span class="p">(</span><span class="n">R</span><span class="p">):</span>
                <span class="c1"># rotation matrix is identity</span>
                <span class="n">skw</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">))</span>
                <span class="n">v</span> <span class="o">=</span> <span class="n">t</span>
                <span class="n">theta</span> <span class="o">=</span> <span class="mi">1</span>
            <span class="k">else</span><span class="p">:</span>
                <span class="n">S</span> <span class="o">=</span> <span class="n">trlog</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">False</span><span class="p">)</span>  <span class="c1"># recurse</span>
                <span class="n">w</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">vex</span><span class="p">(</span><span class="n">S</span><span class="p">)</span>
                <span class="n">theta</span> <span class="o">=</span> <span class="n">vec</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">w</span><span class="p">)</span>
                <span class="n">skw</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">skew</span><span class="p">(</span><span class="n">w</span> <span class="o">/</span> <span class="n">theta</span><span class="p">)</span>
                <span class="n">Ginv</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span> <span class="o">/</span> <span class="n">theta</span> <span class="o">-</span> <span class="n">skw</span> <span class="o">/</span> <span class="mi">2</span> <span class="o">+</span> <span class="p">(</span><span class="mi">1</span> <span class="o">/</span> <span class="n">theta</span> <span class="o">-</span> <span class="mi">1</span> <span class="o">/</span> <span class="n">np</span><span class="o">.</span><span class="n">tan</span><span class="p">(</span><span class="n">theta</span> <span class="o">/</span> <span class="mi">2</span><span class="p">)</span> <span class="o">/</span> <span class="mi">2</span><span class="p">)</span> <span class="o">*</span> <span class="n">skw</span> <span class="o">@</span> <span class="n">skw</span>
                <span class="n">v</span> <span class="o">=</span> <span class="n">Ginv</span> <span class="o">@</span> <span class="n">t</span>
            <span class="k">return</span> <span class="n">trn</span><span class="o">.</span><span class="n">rt2m</span><span class="p">(</span><span class="n">skw</span><span class="p">,</span> <span class="n">v</span><span class="p">)</span> <span class="o">*</span> <span class="n">theta</span>

    <span class="k">elif</span> <span class="n">isrot</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="n">check</span><span class="p">):</span>
        <span class="c1"># deal with rotation matrix</span>
        <span class="n">R</span> <span class="o">=</span> <span class="n">T</span>
        <span class="k">if</span> <span class="n">trn</span><span class="o">.</span><span class="n">iseye</span><span class="p">(</span><span class="n">R</span><span class="p">):</span>
            <span class="c1"># matrix is identity</span>
            <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">))</span>
        <span class="k">elif</span> <span class="nb">abs</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">trace</span><span class="p">(</span><span class="n">R</span><span class="p">)</span> <span class="o">+</span> <span class="mi">1</span><span class="p">)</span> <span class="o">&lt;</span> <span class="mi">100</span> <span class="o">*</span> <span class="n">_eps</span><span class="p">:</span>
            <span class="c1"># check for trace = -1</span>
            <span class="c1">#   rotation by +/- pi, +/- 3pi etc.</span>
            <span class="n">diagonal</span> <span class="o">=</span> <span class="n">R</span><span class="o">.</span><span class="n">diagonal</span><span class="p">()</span>
            <span class="n">k</span> <span class="o">=</span> <span class="n">diagonal</span><span class="o">.</span><span class="n">argmax</span><span class="p">()</span>
            <span class="n">mx</span> <span class="o">=</span> <span class="n">diagonal</span><span class="p">[</span><span class="n">k</span><span class="p">]</span>
            <span class="n">I</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span>
            <span class="n">col</span> <span class="o">=</span> <span class="n">R</span><span class="p">[:,</span> <span class="n">k</span><span class="p">]</span> <span class="o">+</span> <span class="n">I</span><span class="p">[:,</span> <span class="n">k</span><span class="p">]</span>
            <span class="n">w</span> <span class="o">=</span> <span class="n">col</span> <span class="o">/</span> <span class="n">np</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="mi">2</span> <span class="o">*</span> <span class="p">(</span><span class="mi">1</span> <span class="o">+</span> <span class="n">mx</span><span class="p">))</span>
            <span class="n">theta</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">pi</span>
            <span class="k">return</span> <span class="n">trn</span><span class="o">.</span><span class="n">skew</span><span class="p">(</span><span class="n">w</span> <span class="o">*</span> <span class="n">theta</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="c1"># general case</span>
            <span class="n">theta</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">arccos</span><span class="p">((</span><span class="n">np</span><span class="o">.</span><span class="n">trace</span><span class="p">(</span><span class="n">R</span><span class="p">)</span> <span class="o">-</span> <span class="mi">1</span><span class="p">)</span> <span class="o">/</span> <span class="mi">2</span><span class="p">)</span>
            <span class="n">skw</span> <span class="o">=</span> <span class="p">(</span><span class="n">R</span> <span class="o">-</span> <span class="n">R</span><span class="o">.</span><span class="n">T</span><span class="p">)</span> <span class="o">/</span> <span class="mi">2</span> <span class="o">/</span> <span class="n">np</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>
            <span class="k">return</span> <span class="n">skw</span> <span class="o">*</span> <span class="n">theta</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s2">&quot;Expect SO(3) or SE(3) matrix&quot;</span><span class="p">)</span></div>

<span class="c1"># ---------------------------------------------------------------------------------------#</span>


<div class="viewcode-block" id="trexp"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.trexp">[docs]</a><span class="k">def</span> <span class="nf">trexp</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="n">theta</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Exponential of so(3) or se(3) matrix</span>

<span class="sd">    :param S: so(3), se(3) matrix or equivalent velctor</span>
<span class="sd">    :type T: numpy.ndarray, shape=(3,3), (3,), (4,4), or (6,)</span>
<span class="sd">    :param theta: motion</span>
<span class="sd">    :type theta: float</span>
<span class="sd">    :return: 3x3 or 4x4 matrix exponential in SO(3) or SE(3)</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(3,3) or (4,4)</span>

<span class="sd">    An efficient closed-form solution of the matrix exponential for arguments</span>
<span class="sd">    that are so(3) or se(3).</span>

<span class="sd">    For so(3) the results is an SO(3) rotation matrix:</span>

<span class="sd">    - ``trexp(S)`` is the matrix exponential of the so(3) element ``S`` which is a 3x3</span>
<span class="sd">       skew-symmetric matrix.</span>
<span class="sd">    - ``trexp(S, THETA)`` as above but for an so(3) motion of S*THETA, where ``S`` is</span>
<span class="sd">      unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude</span>
<span class="sd">      given by ``THETA``.</span>
<span class="sd">    - ``trexp(W)`` is the matrix exponential of the so(3) element ``W`` expressed as</span>
<span class="sd">      a 3-vector (array_like).</span>
<span class="sd">    - ``trexp(W, THETA)`` as above but for an so(3) motion of W*THETA where ``W`` is a</span>
<span class="sd">      unit-norm vector representing a rotation axis and a rotation magnitude</span>
<span class="sd">      given by ``THETA``. ``W`` is expressed as a 3-vector (array_like).</span>


<span class="sd">    For se(3) the results is an SE(3) homogeneous transformation matrix:</span>

<span class="sd">    - ``trexp(SIGMA)`` is the matrix exponential of the se(3) element ``SIGMA`` which is</span>
<span class="sd">      a 4x4 augmented skew-symmetric matrix.</span>
<span class="sd">    - ``trexp(SIGMA, THETA)`` as above but for an se(3) motion of SIGMA*THETA, where ``SIGMA``</span>
<span class="sd">      must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric</span>
<span class="sd">      matrix.</span>
<span class="sd">    - ``trexp(TW)`` is the matrix exponential of the se(3) element ``TW`` represented as</span>
<span class="sd">      a 6-vector which can be considered a screw motion.</span>
<span class="sd">    - ``trexp(TW, THETA)`` as above but for an se(3) motion of TW*THETA, where ``TW``</span>
<span class="sd">      must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric</span>
<span class="sd">      matrix.</span>

<span class="sd">     :seealso: :func:`~trlog, :func:`~spatialmath.base.transforms2d.trexp2`</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="k">if</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="p">(</span><span class="mi">4</span><span class="p">,</span> <span class="mi">4</span><span class="p">))</span> <span class="ow">or</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">isvector</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="mi">6</span><span class="p">):</span>
        <span class="c1"># se(3) case</span>
        <span class="k">if</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="p">(</span><span class="mi">4</span><span class="p">,</span> <span class="mi">4</span><span class="p">)):</span>
            <span class="c1"># augmentented skew matrix</span>
            <span class="n">tw</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">vexa</span><span class="p">(</span><span class="n">S</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="c1"># 6 vector</span>
            <span class="n">tw</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">S</span><span class="p">)</span>

        <span class="k">if</span> <span class="n">vec</span><span class="o">.</span><span class="n">iszerovec</span><span class="p">(</span><span class="n">tw</span><span class="p">):</span>
            <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">4</span><span class="p">)</span>
        
        <span class="k">if</span> <span class="n">theta</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
            <span class="p">(</span><span class="n">tw</span><span class="p">,</span> <span class="n">theta</span><span class="p">)</span> <span class="o">=</span> <span class="n">vec</span><span class="o">.</span><span class="n">unittwist_norm</span><span class="p">(</span><span class="n">tw</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">if</span> <span class="n">theta</span> <span class="o">==</span> <span class="mi">0</span><span class="p">:</span>
                <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">4</span><span class="p">)</span>
            <span class="k">else</span><span class="p">:</span>
                <span class="k">assert</span> <span class="n">vec</span><span class="o">.</span><span class="n">isunittwist</span><span class="p">(</span><span class="n">tw</span><span class="p">),</span> <span class="s1">&#39;If theta is specified S must be a unit twist&#39;</span>

        <span class="n">t</span> <span class="o">=</span> <span class="n">tw</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">3</span><span class="p">]</span>
        <span class="n">w</span> <span class="o">=</span> <span class="n">tw</span><span class="p">[</span><span class="mi">3</span><span class="p">:</span><span class="mi">6</span><span class="p">]</span>

        <span class="n">R</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">_rodrigues</span><span class="p">(</span><span class="n">w</span><span class="p">,</span> <span class="n">theta</span><span class="p">)</span>

        <span class="n">skw</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">skew</span><span class="p">(</span><span class="n">w</span><span class="p">)</span>
        <span class="n">V</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span> <span class="o">*</span> <span class="n">theta</span> <span class="o">+</span> <span class="p">(</span><span class="mf">1.0</span> <span class="o">-</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">theta</span><span class="p">))</span> <span class="o">*</span> <span class="n">skw</span> <span class="o">+</span> <span class="p">(</span><span class="n">theta</span> <span class="o">-</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">theta</span><span class="p">))</span> <span class="o">*</span> <span class="n">skw</span> <span class="o">@</span> <span class="n">skw</span>

        <span class="k">return</span> <span class="n">trn</span><span class="o">.</span><span class="n">rt2tr</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">V</span><span class="nd">@t</span><span class="p">)</span>

    <span class="k">elif</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="p">(</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">))</span> <span class="ow">or</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">isvector</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="mi">3</span><span class="p">):</span>
        <span class="c1"># so(3) case</span>
        <span class="k">if</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="p">(</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">)):</span>
            <span class="c1"># skew symmetric matrix</span>
            <span class="n">w</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">vex</span><span class="p">(</span><span class="n">S</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="c1"># 3 vector</span>
            <span class="n">w</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">S</span><span class="p">)</span>

        <span class="k">if</span> <span class="n">theta</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span><span class="p">:</span>
            <span class="k">assert</span> <span class="n">vec</span><span class="o">.</span><span class="n">isunitvec</span><span class="p">(</span><span class="n">w</span><span class="p">),</span> <span class="s1">&#39;If theta is specified S must be a unit twist&#39;</span>

        <span class="c1"># do Rodrigues&#39; formula for rotation</span>
        <span class="k">return</span> <span class="n">trn</span><span class="o">.</span><span class="n">_rodrigues</span><span class="p">(</span><span class="n">w</span><span class="p">,</span> <span class="n">theta</span><span class="p">)</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s2">&quot; First argument must be SO(3), 3-vector, SE(3) or 6-vector&quot;</span><span class="p">)</span></div>

<div class="viewcode-block" id="trnorm"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.trnorm">[docs]</a><span class="k">def</span> <span class="nf">trnorm</span><span class="p">(</span><span class="n">T</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Normalize an SO(3) or SE(3) matrix</span>
<span class="sd">    </span>
<span class="sd">    :param T: SO(3) or SE(3) matrix</span>
<span class="sd">    :type T1: np.ndarray, shape=(3,3) or (4,4)</span>
<span class="sd">    :param T1: second SE(3) matrix</span>
<span class="sd">    :return: SO(3) or SE(3) matrix</span>
<span class="sd">    :rtype: np.ndarray, shape=(3,3) or (4,4)</span>

<span class="sd">    - ``trnorm(R)`` is guaranteed to be a proper orthogonal matrix rotation</span>
<span class="sd">      matrix (3x3) which is &quot;close&quot; to the input matrix R (3x3). If R</span>
<span class="sd">      = [N,O,A] the O and A vectors are made unit length and the normal vector</span>
<span class="sd">      is formed from N = O x A, and then we ensure that O and A are orthogonal</span>
<span class="sd">      by O = A x N.</span>
<span class="sd">    </span>
<span class="sd">    - ``trnorm(T)`` as above but the rotational submatrix of the homogeneous</span>
<span class="sd">      transformation T (4x4) is normalised while the translational part is</span>
<span class="sd">      unchanged.</span>

<span class="sd">    Notes:</span>
<span class="sd">        </span>
<span class="sd">    - Only the direction of A (the z-axis) is unchanged.</span>
<span class="sd">    - Used to prevent finite word length arithmetic causing transforms to </span>
<span class="sd">      become &#39;unnormalized&#39;.</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="k">assert</span> <span class="n">ishom</span><span class="p">(</span><span class="n">T</span><span class="p">)</span> <span class="ow">or</span> <span class="n">isrot</span><span class="p">(</span><span class="n">T</span><span class="p">),</span> <span class="s1">&#39;expecting 3x3 or 4x4 hom xform&#39;</span>
    
    <span class="n">o</span> <span class="o">=</span> <span class="n">T</span><span class="p">[:</span><span class="mi">3</span><span class="p">,</span><span class="mi">1</span><span class="p">]</span>
    <span class="n">a</span> <span class="o">=</span> <span class="n">T</span><span class="p">[:</span><span class="mi">3</span><span class="p">,</span><span class="mi">2</span><span class="p">]</span>
    
    <span class="n">n</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">o</span><span class="p">,</span> <span class="n">a</span><span class="p">)</span>        <span class="c1"># N = O x A</span>
    <span class="n">o</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">a</span><span class="p">,</span> <span class="n">n</span><span class="p">)</span>        <span class="c1"># (a)];</span>
    <span class="n">R</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">stack</span><span class="p">((</span><span class="n">vec</span><span class="o">.</span><span class="n">unitvec</span><span class="p">(</span><span class="n">n</span><span class="p">),</span> <span class="n">vec</span><span class="o">.</span><span class="n">unitvec</span><span class="p">(</span><span class="n">o</span><span class="p">),</span> <span class="n">vec</span><span class="o">.</span><span class="n">unitvec</span><span class="p">(</span><span class="n">a</span><span class="p">)),</span> <span class="n">axis</span><span class="o">=</span><span class="mi">1</span><span class="p">)</span>
    
    <span class="k">if</span> <span class="n">ishom</span><span class="p">(</span><span class="n">T</span><span class="p">):</span>
        <span class="k">return</span> <span class="n">trn</span><span class="o">.</span><span class="n">rt2tr</span><span class="p">(</span> <span class="n">R</span><span class="p">,</span> <span class="n">T</span><span class="p">[:</span><span class="mi">3</span><span class="p">,</span><span class="mi">3</span><span class="p">]</span> <span class="p">)</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">return</span> <span class="n">R</span></div>
    
<div class="viewcode-block" id="trinterp"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.trinterp">[docs]</a><span class="k">def</span> <span class="nf">trinterp</span><span class="p">(</span><span class="n">T0</span><span class="p">,</span> <span class="n">T1</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">s</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Interpolate SE(3) matrices</span>
<span class="sd">    </span>
<span class="sd">    :param T0: first SE(3) matrix</span>
<span class="sd">    :type T0: np.ndarray, shape=(4,4)</span>
<span class="sd">    :param T1: second SE(3) matrix</span>
<span class="sd">    :type T1: np.ndarray, shape=(4,4)</span>
<span class="sd">    :param s: interpolation coefficient, range 0 to 1</span>
<span class="sd">    :type s: float</span>
<span class="sd">    :return: SE(3) matrix</span>
<span class="sd">    :rtype: np.ndarray, shape=(4,4)</span>
<span class="sd">    </span>
<span class="sd">    - ``trinterp(T0, T1, S)`` is a homogeneous transform (4x4) interpolated</span>
<span class="sd">      between T0 when S=0 and T1 when S=1.  T0 and T1 are both homogeneous</span>
<span class="sd">      transforms (4x4).</span>
<span class="sd">    </span>
<span class="sd">    - ``trinterp(T1, S)`` as above but interpolated between the identity matrix</span>
<span class="sd">      when S=0 to T1 when S=1.</span>
<span class="sd">    </span>
<span class="sd">    </span>
<span class="sd">    Notes:</span>
<span class="sd">        </span>
<span class="sd">    - Rotation is interpolated using quaternion spherical linear interpolation (slerp).</span>

<span class="sd">    :seealso: :func:`spatialmath.base.quaternions.slerp`, :func:`~spatialmath.base.transforms3d.trinterp2`</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="k">assert</span> <span class="mi">0</span> <span class="o">&lt;=</span> <span class="n">s</span> <span class="o">&lt;=</span> <span class="mi">1</span><span class="p">,</span> <span class="s1">&#39;s outside interval [0,1]&#39;</span>
    
    <span class="k">if</span> <span class="n">T1</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
        <span class="c1">#	TRINTERP(T, s)</span>
        
        <span class="n">q0</span> <span class="o">=</span> <span class="n">quat</span><span class="o">.</span><span class="n">r2q</span><span class="p">(</span><span class="n">trn</span><span class="o">.</span><span class="n">t2r</span><span class="p">(</span><span class="n">T0</span><span class="p">))</span>
        <span class="n">p0</span> <span class="o">=</span> <span class="n">transl</span><span class="p">(</span><span class="n">T0</span><span class="p">)</span>
        
        <span class="n">qr</span> <span class="o">=</span> <span class="n">quat</span><span class="o">.</span><span class="n">slerp</span><span class="p">(</span><span class="n">quat</span><span class="o">.</span><span class="n">eye</span><span class="p">(),</span> <span class="n">q0</span><span class="p">,</span> <span class="n">s</span><span class="p">)</span>
        <span class="n">pr</span> <span class="o">=</span> <span class="n">s</span> <span class="o">*</span> <span class="n">p0</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="c1">#	TRINTERP(T0, T1, s)</span>
    
        <span class="n">q0</span> <span class="o">=</span> <span class="n">quat</span><span class="o">.</span><span class="n">r2q</span><span class="p">(</span><span class="n">trn</span><span class="o">.</span><span class="n">t2r</span><span class="p">(</span><span class="n">T0</span><span class="p">))</span>
        <span class="n">q1</span> <span class="o">=</span> <span class="n">quat</span><span class="o">.</span><span class="n">r2q</span><span class="p">(</span><span class="n">trn</span><span class="o">.</span><span class="n">t2r</span><span class="p">(</span><span class="n">T1</span><span class="p">))</span>
        
        <span class="n">p0</span> <span class="o">=</span> <span class="n">transl</span><span class="p">(</span><span class="n">T0</span><span class="p">)</span>
        <span class="n">p1</span> <span class="o">=</span> <span class="n">transl</span><span class="p">(</span><span class="n">T1</span><span class="p">)</span>
        
        <span class="n">qr</span> <span class="o">=</span> <span class="n">quat</span><span class="o">.</span><span class="n">slerp</span><span class="p">(</span><span class="n">q0</span><span class="p">,</span> <span class="n">q1</span><span class="p">,</span> <span class="n">s</span><span class="p">)</span>
        <span class="n">pr</span> <span class="o">=</span> <span class="n">p0</span> <span class="o">*</span> <span class="p">(</span><span class="mi">1</span> <span class="o">-</span> <span class="n">s</span><span class="p">)</span> <span class="o">+</span> <span class="n">s</span> <span class="o">*</span> <span class="n">p1</span><span class="p">;</span>
        
    <span class="k">return</span> <span class="n">trn</span><span class="o">.</span><span class="n">rt2tr</span><span class="p">(</span><span class="n">quat</span><span class="o">.</span><span class="n">q2r</span><span class="p">(</span><span class="n">qr</span><span class="p">),</span> <span class="n">pr</span><span class="p">)</span></div>

<div class="viewcode-block" id="delta2tr"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.delta2tr">[docs]</a><span class="k">def</span> <span class="nf">delta2tr</span><span class="p">(</span><span class="n">d</span><span class="p">):</span>
    <span class="sa">r</span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Convert differential motion to SE(3)</span>
<span class="sd">    </span>
<span class="sd">    :param d: differential motion as a 6-vector</span>
<span class="sd">    :type d: array_like</span>
<span class="sd">    :return: SE(3) matrix</span>
<span class="sd">    :rtype: np.ndarray, shape=(4,4)</span>

<span class="sd">    ``T = delta2tr(d)`` is an SE(3) matrix representing differential </span>
<span class="sd">    motion :math:`d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z`.</span>

<span class="sd">    Reference: Robotics, Vision &amp; Control: Second Edition, P. Corke, Springer 2016; p67.</span>
<span class="sd">    </span>
<span class="sd">    :seealso: :func:`~tr2delta`</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">4</span><span class="p">,</span><span class="mi">4</span><span class="p">)</span> <span class="o">+</span> <span class="n">trn</span><span class="o">.</span><span class="n">skewa</span><span class="p">(</span><span class="n">d</span><span class="p">)</span></div>
    
<div class="viewcode-block" id="trinv"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.trinv">[docs]</a><span class="k">def</span> <span class="nf">trinv</span><span class="p">(</span><span class="n">T</span><span class="p">):</span>
    <span class="sa">r</span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Invert an SE(3) matrix</span>
<span class="sd">    </span>
<span class="sd">    :param T: an SE(3) matrix</span>
<span class="sd">    :type T: np.ndarray, shape=(4,4)</span>
<span class="sd">    :return: SE(3) matrix</span>
<span class="sd">    :rtype: np.ndarray, shape=(4,4)</span>

<span class="sd">    Computes an efficient inverse of an SE(3) matrix:</span>
<span class="sd">    </span>
<span class="sd">    :math:`\begin{pmatrix} {\bf R} &amp; t \\ 0\,0\,0 &amp; 1 \end{pmatrix}^{-1} =  \begin{pmatrix} {\bf R}^T &amp; -{\bf R}^T t \\ 0\,0\, 0 &amp; 1 \end{pmatrix}`</span>

<span class="sd">    &quot;&quot;&quot;</span>
    <span class="k">assert</span> <span class="n">ishom</span><span class="p">(</span><span class="n">T</span><span class="p">),</span> <span class="s1">&#39;expecting SE(3) matrix&#39;</span>
    <span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">t</span><span class="p">)</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">tr2rt</span><span class="p">(</span><span class="n">T</span><span class="p">)</span>
    <span class="k">return</span> <span class="n">trn</span><span class="o">.</span><span class="n">rt2tr</span><span class="p">(</span><span class="n">R</span><span class="o">.</span><span class="n">T</span><span class="p">,</span> <span class="o">-</span><span class="n">R</span><span class="o">.</span><span class="n">T</span><span class="nd">@t</span><span class="p">)</span></div>

<div class="viewcode-block" id="tr2delta"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.tr2delta">[docs]</a><span class="k">def</span> <span class="nf">tr2delta</span><span class="p">(</span><span class="n">T0</span><span class="p">,</span> <span class="n">T1</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Difference of SE(3) matrices as differential motion</span>
<span class="sd">    </span>
<span class="sd">    :param T0: first SE(3) matrix</span>
<span class="sd">    :type T0: np.ndarray, shape=(4,4)</span>
<span class="sd">    :param T1: second SE(3) matrix</span>
<span class="sd">    :type T1: np.ndarray, shape=(4,4)</span>
<span class="sd">    :return: Sdifferential motion as a 6-vector</span>
<span class="sd">    :rtype: np.ndarray, shape=(6,)</span>


<span class="sd">    - ``tr2delta(T0, T1)`` is the differential motion (6x1) corresponding to </span>
<span class="sd">      infinitessimal motion (in the T0 frame) from pose T0 to T1 which are SE(3) matrices.</span>

<span class="sd">    - ``tr2delta(T)`` as above but the motion is from the world frame to the pose represented by T.</span>

<span class="sd">    The vector :math:`d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z`</span>
<span class="sd">    represents infinitessimal translation and rotation, and is an approximation to the </span>
<span class="sd">    instantaneous spatial velocity multiplied by time step.</span>

<span class="sd">    Notes:</span>
<span class="sd">        </span>
<span class="sd">    - D is only an approximation to the motion T, and assumes</span>
<span class="sd">      that T0 ~ T1 or T ~ eye(4,4).</span>
<span class="sd">    - Can be considered as an approximation to the effect of spatial velocity over a</span>
<span class="sd">      a time interval, average spatial velocity multiplied by time.</span>
<span class="sd">    </span>
<span class="sd">    Reference: Robotics, Vision &amp; Control: Second Edition, P. Corke, Springer 2016; p67.</span>

<span class="sd">    :seealso: :func:`~delta2tr`</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="k">if</span> <span class="n">T1</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
        <span class="c1"># tr2delta(T)</span>
        
        <span class="k">assert</span> <span class="n">ishom</span><span class="p">(</span><span class="n">T0</span><span class="p">),</span> <span class="s1">&#39;expecting SE(3) matrix&#39;</span>
        <span class="n">Td</span> <span class="o">=</span> <span class="n">T0</span>
        
    <span class="k">else</span><span class="p">:</span>
        <span class="c1">#  incremental transformation from T0 to T1 in the T0 frame</span>
        <span class="n">Td</span> <span class="o">=</span> <span class="n">trinv</span><span class="p">(</span><span class="n">T0</span><span class="p">)</span> <span class="o">@</span> <span class="n">T1</span>
    
    <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">r_</span><span class="p">[</span><span class="n">transl</span><span class="p">(</span><span class="n">Td</span><span class="p">),</span> <span class="n">trn</span><span class="o">.</span><span class="n">vex</span><span class="p">(</span><span class="n">trn</span><span class="o">.</span><span class="n">t2r</span><span class="p">(</span><span class="n">Td</span><span class="p">)</span> <span class="o">-</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">))]</span></div>

<div class="viewcode-block" id="tr2jac"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.tr2jac">[docs]</a><span class="k">def</span> <span class="nf">tr2jac</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">samebody</span><span class="o">=</span><span class="kc">False</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    SE(3) adjoint</span>
<span class="sd">    </span>
<span class="sd">    :param T: an SE(3) matrix</span>
<span class="sd">    :type T: np.ndarray, shape=(4,4)</span>
<span class="sd">    :return: adjoint matrix</span>
<span class="sd">    :rtype: np.ndarray, shape=(6,6)</span>
<span class="sd">    </span>
<span class="sd">    Computes an adjoint matrix that maps spatial velocity between two frames defined by</span>
<span class="sd">    an SE(3) matrix.  It acts like a Jacobian matrix.</span>
<span class="sd">    </span>
<span class="sd">    - ``tr2jac(T)`` is a Jacobian matrix (6x6) that maps spatial velocity or</span>
<span class="sd">      differential motion from frame {A} to frame {B} where the pose of {B}</span>
<span class="sd">      relative to {A} is represented by the homogeneous transform T = :math:`{}^A {\bf T}_B`.</span>

<span class="sd">    - ``tr2jac(T, True)`` as above but for the case when frame {A} to frame {B} are both</span>
<span class="sd">      attached to the same moving body.</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="k">assert</span> <span class="n">ishom</span><span class="p">(</span><span class="n">T</span><span class="p">),</span> <span class="s1">&#39;expecting an SE(3) matrix&#39;</span>
    <span class="n">Z</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">3</span><span class="p">,</span><span class="mi">3</span><span class="p">))</span>
    	
    <span class="k">if</span> <span class="n">samebody</span><span class="p">:</span>
        <span class="p">(</span><span class="n">R</span><span class="p">,</span><span class="n">t</span><span class="p">)</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">tr2rt</span><span class="p">(</span><span class="n">T</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">block</span><span class="p">([[</span><span class="n">R</span><span class="o">.</span><span class="n">T</span><span class="p">,</span> <span class="p">(</span><span class="n">trn</span><span class="o">.</span><span class="n">skew</span><span class="p">(</span><span class="n">t</span><span class="p">)</span><span class="nd">@R</span><span class="p">)</span><span class="o">.</span><span class="n">T</span><span class="p">],</span> <span class="p">[</span><span class="n">Z</span><span class="p">,</span> <span class="n">R</span><span class="o">.</span><span class="n">T</span><span class="p">]])</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="n">R</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">t2r</span><span class="p">(</span><span class="n">T</span><span class="p">);</span>
        <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">block</span><span class="p">([[</span><span class="n">R</span><span class="o">.</span><span class="n">T</span><span class="p">,</span> <span class="n">Z</span><span class="p">],</span> <span class="p">[</span><span class="n">Z</span><span class="p">,</span> <span class="n">R</span><span class="o">.</span><span class="n">T</span><span class="p">]])</span></div>


<div class="viewcode-block" id="trprint"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.trprint">[docs]</a><span class="k">def</span> <span class="nf">trprint</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">orient</span><span class="o">=</span><span class="s1">&#39;rpy/zyx&#39;</span><span class="p">,</span> <span class="n">label</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">file</span><span class="o">=</span><span class="n">sys</span><span class="o">.</span><span class="n">stdout</span><span class="p">,</span> <span class="n">fmt</span><span class="o">=</span><span class="s1">&#39;</span><span class="si">{:8.2g}</span><span class="s1">&#39;</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s1">&#39;deg&#39;</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Compact display of SO(3) or SE(3) matrices</span>

<span class="sd">    :param T: matrix to format</span>
<span class="sd">    :type T: numpy.ndarray, shape=(3,3) or (4,4)</span>
<span class="sd">    :param label: text label to put at start of line</span>
<span class="sd">    :type label: str</span>
<span class="sd">    :param orient: 3-angle convention to use</span>
<span class="sd">    :type orient: str</span>
<span class="sd">    :param file: file to write formatted string to. [default, stdout]</span>
<span class="sd">    :type file: str</span>
<span class="sd">    :param fmt: conversion format for each number</span>
<span class="sd">    :type fmt: str</span>
<span class="sd">    :param unit: angular units: &#39;rad&#39; [default], or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :return: optional formatted string</span>
<span class="sd">    :rtype: str</span>

<span class="sd">    The matrix is formatted and written to ``file`` or if ``file=None`` then the</span>
<span class="sd">    string is returned.</span>

<span class="sd">   - ``trprint(R)`` displays the SO(3) rotation matrix in a compact</span>
<span class="sd">      single-line format:</span>

<span class="sd">        [LABEL:] ORIENTATION UNIT</span>

<span class="sd">    - ``trprint(T)`` displays the SE(3) homogoneous transform in a compact</span>
<span class="sd">      single-line format:</span>

<span class="sd">        [LABEL:] [t=X, Y, Z;] ORIENTATION UNIT</span>

<span class="sd">    Orientation is expressed in one of several formats:</span>

<span class="sd">    - &#39;rpy/zyx&#39; roll-pitch-yaw angles in ZYX axis order [default]</span>
<span class="sd">    - &#39;rpy/yxz&#39; roll-pitch-yaw angles in YXZ axis order</span>
<span class="sd">    - &#39;rpy/zyx&#39; roll-pitch-yaw angles in ZYX axis order</span>
<span class="sd">    - &#39;eul&#39; Euler angles in ZYZ axis order</span>
<span class="sd">    - &#39;angvec&#39; angle and axis</span>


<span class="sd">    Example:</span>

<span class="sd">    &gt;&gt;&gt; T = transl(1,2,3) @ rpy2tr(10, 20, 30, &#39;deg&#39;)</span>
<span class="sd">    &gt;&gt;&gt; trprint(T, file=None, label=&#39;T&#39;)</span>
<span class="sd">    &#39;T: t =        1,        2,        3; rpy/zyx =       10,       20,       30 deg&#39;</span>
<span class="sd">    &gt;&gt;&gt; trprint(T, file=None, label=&#39;T&#39;, orient=&#39;angvec&#39;)</span>
<span class="sd">    &#39;T: t =        1,        2,        3; angvec = (      56 deg |     0.12,     0.62,     0.78)&#39;</span>
<span class="sd">    &gt;&gt;&gt; trprint(T, file=None, label=&#39;T&#39;, orient=&#39;angvec&#39;, fmt=&#39;{:8.4g}&#39;)</span>
<span class="sd">    &#39;T: t =        1,        2,        3; angvec = (   56.04 deg |    0.124,   0.6156,   0.7782)&#39;</span>

<span class="sd">    Notes:</span>

<span class="sd">     - If the &#39;rpy&#39; option is selected, then the particular angle sequence can be</span>
<span class="sd">       specified with the options &#39;xyz&#39; or &#39;yxz&#39; which are passed through to ``tr2rpy``.</span>
<span class="sd">       &#39;zyx&#39; is the default.</span>
<span class="sd">     - Default formatting is for readable columns of data</span>

<span class="sd">    :seealso: :func:`~spatialmath.base.transforms2d.trprint2`, :func:`~tr2eul`, :func:`~tr2rpy`, :func:`~tr2angvec`</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="n">s</span> <span class="o">=</span> <span class="s1">&#39;&#39;</span>

    <span class="k">if</span> <span class="n">label</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span><span class="p">:</span>
        <span class="n">s</span> <span class="o">+=</span> <span class="s1">&#39;</span><span class="si">{:s}</span><span class="s1">: &#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">label</span><span class="p">)</span>

    <span class="c1"># print the translational part if it exists</span>
    <span class="k">if</span> <span class="n">ishom</span><span class="p">(</span><span class="n">T</span><span class="p">):</span>
        <span class="n">s</span> <span class="o">+=</span> <span class="s1">&#39;t = </span><span class="si">{}</span><span class="s1">;&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">_vec2s</span><span class="p">(</span><span class="n">fmt</span><span class="p">,</span> <span class="n">transl</span><span class="p">(</span><span class="n">T</span><span class="p">)))</span>

    <span class="c1"># print the angular part in various representations</span>

    <span class="n">a</span> <span class="o">=</span> <span class="n">orient</span><span class="o">.</span><span class="n">split</span><span class="p">(</span><span class="s1">&#39;/&#39;</span><span class="p">)</span>
    <span class="k">if</span> <span class="n">a</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">==</span> <span class="s1">&#39;rpy&#39;</span><span class="p">:</span>
        <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="n">a</span><span class="p">)</span> <span class="o">==</span> <span class="mi">2</span><span class="p">:</span>
            <span class="n">seq</span> <span class="o">=</span> <span class="n">a</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">seq</span> <span class="o">=</span> <span class="kc">None</span>
        <span class="n">angles</span> <span class="o">=</span> <span class="n">tr2rpy</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">order</span><span class="o">=</span><span class="n">seq</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="n">unit</span><span class="p">)</span>
        <span class="n">s</span> <span class="o">+=</span> <span class="s1">&#39; </span><span class="si">{}</span><span class="s1"> = </span><span class="si">{}</span><span class="s1"> </span><span class="si">{}</span><span class="s1">&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">orient</span><span class="p">,</span> <span class="n">_vec2s</span><span class="p">(</span><span class="n">fmt</span><span class="p">,</span> <span class="n">angles</span><span class="p">),</span> <span class="n">unit</span><span class="p">)</span>

    <span class="k">elif</span> <span class="n">a</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span><span class="o">.</span><span class="n">startswith</span><span class="p">(</span><span class="s1">&#39;eul&#39;</span><span class="p">):</span>
        <span class="n">angles</span> <span class="o">=</span> <span class="n">tr2eul</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">unit</span><span class="p">)</span>
        <span class="n">s</span> <span class="o">+=</span> <span class="s1">&#39; eul = </span><span class="si">{}</span><span class="s1"> </span><span class="si">{}</span><span class="s1">&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">_vec2s</span><span class="p">(</span><span class="n">fmt</span><span class="p">,</span> <span class="n">angles</span><span class="p">),</span> <span class="n">unit</span><span class="p">)</span>

    <span class="k">elif</span> <span class="n">a</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">==</span> <span class="s1">&#39;angvec&#39;</span><span class="p">:</span>
        <span class="k">pass</span>
        <span class="c1"># as a vector and angle</span>
        <span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">v</span><span class="p">)</span> <span class="o">=</span> <span class="n">tr2angvec</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">unit</span><span class="p">)</span>
        <span class="k">if</span> <span class="n">theta</span> <span class="o">==</span> <span class="mi">0</span><span class="p">:</span>
            <span class="n">s</span> <span class="o">+=</span> <span class="s1">&#39; R = nil&#39;</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">s</span> <span class="o">+=</span> <span class="s1">&#39; angvec = (</span><span class="si">{}</span><span class="s1"> </span><span class="si">{}</span><span class="s1"> | </span><span class="si">{}</span><span class="s1">)&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">fmt</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">theta</span><span class="p">),</span> <span class="n">unit</span><span class="p">,</span> <span class="n">_vec2s</span><span class="p">(</span><span class="n">fmt</span><span class="p">,</span> <span class="n">v</span><span class="p">))</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;bad orientation format&#39;</span><span class="p">)</span>

    <span class="k">if</span> <span class="n">file</span><span class="p">:</span>
        <span class="nb">print</span><span class="p">(</span><span class="n">s</span><span class="p">,</span> <span class="n">file</span><span class="o">=</span><span class="n">file</span><span class="p">)</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">return</span> <span class="n">s</span></div>


<span class="k">def</span> <span class="nf">_vec2s</span><span class="p">(</span><span class="n">fmt</span><span class="p">,</span> <span class="n">v</span><span class="p">):</span>
    <span class="n">v</span> <span class="o">=</span> <span class="p">[</span><span class="n">x</span> <span class="k">if</span> <span class="n">np</span><span class="o">.</span><span class="n">abs</span><span class="p">(</span><span class="n">x</span><span class="p">)</span> <span class="o">&gt;</span> <span class="mi">100</span> <span class="o">*</span> <span class="n">_eps</span> <span class="k">else</span> <span class="mf">0.0</span> <span class="k">for</span> <span class="n">x</span> <span class="ow">in</span> <span class="n">v</span><span class="p">]</span>
    <span class="k">return</span> <span class="s1">&#39;, &#39;</span><span class="o">.</span><span class="n">join</span><span class="p">([</span><span class="n">fmt</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">x</span><span class="p">)</span> <span class="k">for</span> <span class="n">x</span> <span class="ow">in</span> <span class="n">v</span><span class="p">])</span>


<span class="k">try</span><span class="p">:</span>
    <span class="kn">import</span> <span class="nn">matplotlib.pyplot</span> <span class="k">as</span> <span class="nn">plt</span>
    <span class="kn">from</span> <span class="nn">mpl_toolkits.mplot3d</span> <span class="kn">import</span> <span class="n">Axes3D</span>
    <span class="n">_matplotlib_exists</span> <span class="o">=</span> <span class="kc">True</span>
    
<span class="k">except</span> <span class="ne">BaseException</span><span class="p">:</span>  <span class="c1"># pragma: no cover</span>
    <span class="k">def</span> <span class="nf">trplot</span><span class="p">(</span><span class="o">*</span><span class="n">args</span><span class="p">,</span> <span class="o">**</span><span class="n">kwargs</span><span class="p">):</span>
        <span class="nb">print</span><span class="p">(</span><span class="s1">&#39;** trplot: no plot produced -- matplotlib not installed&#39;</span><span class="p">)</span>
    <span class="n">_matplotlib_exists</span> <span class="o">=</span> <span class="kc">False</span>
        
<span class="k">if</span> <span class="n">_matplotlib_exists</span><span class="p">:</span>
<div class="viewcode-block" id="trplot"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.trplot">[docs]</a>    <span class="k">def</span> <span class="nf">trplot</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">axes</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">dims</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">color</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">,</span> <span class="n">frame</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">textcolor</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">labels</span><span class="o">=</span><span class="p">[</span><span class="s1">&#39;X&#39;</span><span class="p">,</span> <span class="s1">&#39;Y&#39;</span><span class="p">,</span> <span class="s1">&#39;Z&#39;</span><span class="p">],</span> <span class="n">length</span><span class="o">=</span><span class="mi">1</span><span class="p">,</span> <span class="n">arrow</span><span class="o">=</span><span class="kc">True</span><span class="p">,</span> <span class="n">projection</span><span class="o">=</span><span class="s1">&#39;ortho&#39;</span><span class="p">,</span> <span class="n">rviz</span><span class="o">=</span><span class="kc">False</span><span class="p">,</span> <span class="n">wtl</span><span class="o">=</span><span class="mf">0.2</span><span class="p">,</span> <span class="n">width</span><span class="o">=</span><span class="mi">1</span><span class="p">,</span> <span class="n">d1</span><span class="o">=</span><span class="mf">0.05</span><span class="p">,</span> <span class="n">d2</span><span class="o">=</span><span class="mf">1.15</span><span class="p">,</span> <span class="o">**</span><span class="n">kwargs</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Plot a 3D coordinate frame</span>

<span class="sd">        :param T: an SO(3) or SE(3) pose to be displayed as coordinate frame</span>
<span class="sd">        :type: numpy.ndarray, shape=(3,3) or (4,4)</span>
<span class="sd">        :param axes: the axes to plot into, defaults to current axes</span>
<span class="sd">        :type axes: Axes3D reference</span>
<span class="sd">        :param dims: dimension of plot volume as [xmin, xmax, ymin, ymax,zmin, zmax].</span>
<span class="sd">                     If dims is [min, max] those limits are applied to the x-, y- and z-axes.</span>
<span class="sd">        :type dims: array_like</span>
<span class="sd">        :param color: color of the lines defining the frame</span>
<span class="sd">        :type color: str</span>
<span class="sd">        :param textcolor: color of text labels for the frame, default color of lines above</span>
<span class="sd">        :type textcolor: str</span>
<span class="sd">        :param frame: label the frame, name is shown below the frame and as subscripts on the frame axis labels</span>
<span class="sd">        :type frame: str</span>
<span class="sd">        :param labels: labels for the axes, defaults to X, Y and Z</span>
<span class="sd">        :type labels: 3-tuple of strings</span>
<span class="sd">        :param length: length of coordinate frame axes, default 1</span>
<span class="sd">        :type length: float</span>
<span class="sd">        :param arrow: show arrow heads, default True</span>
<span class="sd">        :type arrow: bool</span>
<span class="sd">        :param wtl: width-to-length ratio for arrows, default 0.2</span>
<span class="sd">        :type wtl: float</span>
<span class="sd">        :param rviz: show Rviz style arrows, default False</span>
<span class="sd">        :type rviz: bool</span>
<span class="sd">        :param projection: 3D projection: ortho [default] or persp</span>
<span class="sd">        :type projection: str</span>
<span class="sd">        :param width: width of lines, default 1</span>
<span class="sd">        :type width: float</span>
<span class="sd">        :param d1: distance of frame axis label text from origin, default 1.15</span>
<span class="sd">        :type d2: distance of frame label text from origin, default 0.05</span>

<span class="sd">        Adds a 3D coordinate frame represented by the SO(3) or SE(3) matrix to the current axes.</span>

<span class="sd">        - If no current figure, one is created</span>
<span class="sd">        - If current figure, but no axes, a 3d Axes is created</span>

<span class="sd">        Examples:</span>

<span class="sd">             trplot(T, frame=&#39;A&#39;)</span>
<span class="sd">             trplot(T, frame=&#39;A&#39;, color=&#39;green&#39;)</span>
<span class="sd">             trplot(T1, &#39;labels&#39;, &#39;NOA&#39;);</span>

<span class="sd">        &quot;&quot;&quot;</span>

        <span class="c1"># TODO</span>
        <span class="c1"># animation</span>
        <span class="c1"># anaglyph</span>

        <span class="c1"># check input types</span>
        <span class="k">if</span> <span class="n">isrot</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">True</span><span class="p">):</span>
            <span class="n">T</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">r2t</span><span class="p">(</span><span class="n">T</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">assert</span> <span class="n">ishom</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">True</span><span class="p">)</span>

        <span class="k">if</span> <span class="n">axes</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
            <span class="c1"># create an axes</span>
            <span class="n">fig</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">gcf</span><span class="p">()</span>
            <span class="k">if</span> <span class="n">fig</span><span class="o">.</span><span class="n">axes</span> <span class="o">==</span> <span class="p">[]:</span>
                <span class="c1"># no axes in the figure, create a 3D axes</span>
                <span class="n">ax</span> <span class="o">=</span> <span class="n">fig</span><span class="o">.</span><span class="n">add_subplot</span><span class="p">(</span><span class="mi">111</span><span class="p">,</span> <span class="n">projection</span><span class="o">=</span><span class="s1">&#39;3d&#39;</span><span class="p">,</span> <span class="n">proj_type</span><span class="o">=</span><span class="n">projection</span><span class="p">)</span>
                <span class="n">ax</span><span class="o">.</span><span class="n">autoscale</span><span class="p">(</span><span class="n">enable</span><span class="o">=</span><span class="kc">True</span><span class="p">,</span> <span class="n">axis</span><span class="o">=</span><span class="s1">&#39;both&#39;</span><span class="p">)</span>

                <span class="c1"># ax.set_aspect(&#39;equal&#39;)</span>
                <span class="n">ax</span><span class="o">.</span><span class="n">set_xlabel</span><span class="p">(</span><span class="n">labels</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span>
                <span class="n">ax</span><span class="o">.</span><span class="n">set_ylabel</span><span class="p">(</span><span class="n">labels</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span>
                <span class="n">ax</span><span class="o">.</span><span class="n">set_zlabel</span><span class="p">(</span><span class="n">labels</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span>
            <span class="k">else</span><span class="p">:</span>
                <span class="c1"># reuse an existing axis</span>
                <span class="n">ax</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">gca</span><span class="p">()</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">ax</span> <span class="o">=</span> <span class="n">axes</span>

        <span class="k">if</span> <span class="n">dims</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span><span class="p">:</span>
            <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="n">dims</span><span class="p">)</span> <span class="o">==</span> <span class="mi">2</span><span class="p">:</span>
                <span class="n">dims</span> <span class="o">=</span> <span class="n">dims</span> <span class="o">*</span> <span class="mi">3</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">set_xlim</span><span class="p">(</span><span class="n">dims</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">2</span><span class="p">])</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">set_ylim</span><span class="p">(</span><span class="n">dims</span><span class="p">[</span><span class="mi">2</span><span class="p">:</span><span class="mi">4</span><span class="p">])</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">set_zlim</span><span class="p">(</span><span class="n">dims</span><span class="p">[</span><span class="mi">4</span><span class="p">:</span><span class="mi">6</span><span class="p">])</span>

        <span class="c1"># create unit vectors in homogeneous form</span>
        <span class="n">o</span> <span class="o">=</span> <span class="n">T</span> <span class="o">@</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>
        <span class="n">x</span> <span class="o">=</span> <span class="n">T</span> <span class="o">@</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span> <span class="o">*</span> <span class="n">length</span>
        <span class="n">y</span> <span class="o">=</span> <span class="n">T</span> <span class="o">@</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span> <span class="o">*</span> <span class="n">length</span>
        <span class="n">z</span> <span class="o">=</span> <span class="n">T</span> <span class="o">@</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span> <span class="o">*</span> <span class="n">length</span>

        <span class="c1"># draw the axes</span>

        <span class="k">if</span> <span class="n">rviz</span><span class="p">:</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">plot</span><span class="p">([</span><span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">0</span><span class="p">]],</span> <span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">1</span><span class="p">]],</span> <span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">2</span><span class="p">]],</span> <span class="n">color</span><span class="o">=</span><span class="s1">&#39;red&#39;</span><span class="p">,</span> <span class="n">linewidth</span><span class="o">=</span><span class="mi">5</span> <span class="o">*</span> <span class="n">width</span><span class="p">)</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">plot</span><span class="p">([</span><span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">0</span><span class="p">]],</span> <span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">1</span><span class="p">]],</span> <span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">2</span><span class="p">]],</span> <span class="n">color</span><span class="o">=</span><span class="s1">&#39;lime&#39;</span><span class="p">,</span> <span class="n">linewidth</span><span class="o">=</span><span class="mi">5</span> <span class="o">*</span> <span class="n">width</span><span class="p">)</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">plot</span><span class="p">([</span><span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">z</span><span class="p">[</span><span class="mi">0</span><span class="p">]],</span> <span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">z</span><span class="p">[</span><span class="mi">1</span><span class="p">]],</span> <span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">z</span><span class="p">[</span><span class="mi">2</span><span class="p">]],</span> <span class="n">color</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">,</span> <span class="n">linewidth</span><span class="o">=</span><span class="mi">5</span> <span class="o">*</span> <span class="n">width</span><span class="p">)</span>
        <span class="k">elif</span> <span class="n">arrow</span><span class="p">:</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">quiver</span><span class="p">(</span><span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">o</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">-</span> <span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">-</span> <span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">-</span> <span class="n">o</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">arrow_length_ratio</span><span class="o">=</span><span class="n">wtl</span><span class="p">,</span> <span class="n">linewidth</span><span class="o">=</span><span class="n">width</span><span class="p">,</span> <span class="n">facecolor</span><span class="o">=</span><span class="n">color</span><span class="p">,</span> <span class="n">edgecolor</span><span class="o">=</span><span class="n">color</span><span class="p">)</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">quiver</span><span class="p">(</span><span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">o</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">-</span> <span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">-</span> <span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">-</span> <span class="n">o</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">arrow_length_ratio</span><span class="o">=</span><span class="n">wtl</span><span class="p">,</span> <span class="n">linewidth</span><span class="o">=</span><span class="n">width</span><span class="p">,</span> <span class="n">facecolor</span><span class="o">=</span><span class="n">color</span><span class="p">,</span> <span class="n">edgecolor</span><span class="o">=</span><span class="n">color</span><span class="p">)</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">quiver</span><span class="p">(</span><span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">o</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">z</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">-</span> <span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">z</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">-</span> <span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">z</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">-</span> <span class="n">o</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">arrow_length_ratio</span><span class="o">=</span><span class="n">wtl</span><span class="p">,</span> <span class="n">linewidth</span><span class="o">=</span><span class="n">width</span><span class="p">,</span> <span class="n">facecolor</span><span class="o">=</span><span class="n">color</span><span class="p">,</span> <span class="n">edgecolor</span><span class="o">=</span><span class="n">color</span><span class="p">)</span>
            <span class="c1"># plot an invisible point at the end of each arrow to allow auto-scaling to work</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">scatter</span><span class="p">(</span><span class="n">xs</span><span class="o">=</span><span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">z</span><span class="p">[</span><span class="mi">0</span><span class="p">]],</span> <span class="n">ys</span><span class="o">=</span><span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">z</span><span class="p">[</span><span class="mi">1</span><span class="p">]],</span> <span class="n">zs</span><span class="o">=</span><span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">z</span><span class="p">[</span><span class="mi">2</span><span class="p">]],</span> <span class="n">s</span><span class="o">=</span><span class="p">[</span><span class="mi">20</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">plot</span><span class="p">([</span><span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">0</span><span class="p">]],</span> <span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">1</span><span class="p">]],</span> <span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">2</span><span class="p">]],</span> <span class="n">color</span><span class="o">=</span><span class="n">color</span><span class="p">,</span> <span class="n">linewidth</span><span class="o">=</span><span class="n">width</span><span class="p">)</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">plot</span><span class="p">([</span><span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">0</span><span class="p">]],</span> <span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">1</span><span class="p">]],</span> <span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">2</span><span class="p">]],</span> <span class="n">color</span><span class="o">=</span><span class="n">color</span><span class="p">,</span> <span class="n">linewidth</span><span class="o">=</span><span class="n">width</span><span class="p">)</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">plot</span><span class="p">([</span><span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">z</span><span class="p">[</span><span class="mi">0</span><span class="p">]],</span> <span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">z</span><span class="p">[</span><span class="mi">1</span><span class="p">]],</span> <span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">z</span><span class="p">[</span><span class="mi">2</span><span class="p">]],</span> <span class="n">color</span><span class="o">=</span><span class="n">color</span><span class="p">,</span> <span class="n">linewidth</span><span class="o">=</span><span class="n">width</span><span class="p">)</span>

        <span class="c1"># label the frame</span>
        <span class="k">if</span> <span class="n">frame</span><span class="p">:</span>
            <span class="k">if</span> <span class="n">textcolor</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span><span class="p">:</span>
                <span class="n">color</span> <span class="o">=</span> <span class="n">textcolor</span>

            <span class="n">o1</span> <span class="o">=</span> <span class="n">T</span> <span class="o">@</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="o">-</span><span class="n">d1</span><span class="p">,</span> <span class="o">-</span><span class="n">d1</span><span class="p">,</span> <span class="o">-</span><span class="n">d1</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">text</span><span class="p">(</span><span class="n">o1</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">o1</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">o1</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="sa">r</span><span class="s1">&#39;$\{&#39;</span> <span class="o">+</span> <span class="n">frame</span> <span class="o">+</span> <span class="sa">r</span><span class="s1">&#39;\}$&#39;</span><span class="p">,</span> <span class="n">color</span><span class="o">=</span><span class="n">color</span><span class="p">,</span> <span class="n">verticalalignment</span><span class="o">=</span><span class="s1">&#39;top&#39;</span><span class="p">,</span> <span class="n">horizontalalignment</span><span class="o">=</span><span class="s1">&#39;center&#39;</span><span class="p">)</span>

            <span class="c1"># add the labels to each axis</span>

            <span class="n">x</span> <span class="o">=</span> <span class="p">(</span><span class="n">x</span> <span class="o">-</span> <span class="n">o</span><span class="p">)</span> <span class="o">*</span> <span class="n">d2</span> <span class="o">+</span> <span class="n">o</span>
            <span class="n">y</span> <span class="o">=</span> <span class="p">(</span><span class="n">y</span> <span class="o">-</span> <span class="n">o</span><span class="p">)</span> <span class="o">*</span> <span class="n">d2</span> <span class="o">+</span> <span class="n">o</span>
            <span class="n">z</span> <span class="o">=</span> <span class="p">(</span><span class="n">z</span> <span class="o">-</span> <span class="n">o</span><span class="p">)</span> <span class="o">*</span> <span class="n">d2</span> <span class="o">+</span> <span class="n">o</span>

            <span class="n">ax</span><span class="o">.</span><span class="n">text</span><span class="p">(</span><span class="n">x</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="s2">&quot;$</span><span class="si">%c</span><span class="s2">_{</span><span class="si">%s</span><span class="s2">}$&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="n">labels</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">frame</span><span class="p">),</span> <span class="n">color</span><span class="o">=</span><span class="n">color</span><span class="p">,</span> <span class="n">horizontalalignment</span><span class="o">=</span><span class="s1">&#39;center&#39;</span><span class="p">,</span> <span class="n">verticalalignment</span><span class="o">=</span><span class="s1">&#39;center&#39;</span><span class="p">)</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">text</span><span class="p">(</span><span class="n">y</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="s2">&quot;$</span><span class="si">%c</span><span class="s2">_{</span><span class="si">%s</span><span class="s2">}$&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="n">labels</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">frame</span><span class="p">),</span> <span class="n">color</span><span class="o">=</span><span class="n">color</span><span class="p">,</span> <span class="n">horizontalalignment</span><span class="o">=</span><span class="s1">&#39;center&#39;</span><span class="p">,</span> <span class="n">verticalalignment</span><span class="o">=</span><span class="s1">&#39;center&#39;</span><span class="p">)</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">text</span><span class="p">(</span><span class="n">z</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">z</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">z</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="s2">&quot;$</span><span class="si">%c</span><span class="s2">_{</span><span class="si">%s</span><span class="s2">}$&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="n">labels</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">frame</span><span class="p">),</span> <span class="n">color</span><span class="o">=</span><span class="n">color</span><span class="p">,</span> <span class="n">horizontalalignment</span><span class="o">=</span><span class="s1">&#39;center&#39;</span><span class="p">,</span> <span class="n">verticalalignment</span><span class="o">=</span><span class="s1">&#39;center&#39;</span><span class="p">)</span></div>

    <span class="kn">from</span> <span class="nn">spatialmath.base</span> <span class="kn">import</span> <span class="n">animate</span> <span class="k">as</span> <span class="n">animate</span>

        
<div class="viewcode-block" id="tranimate"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms3d.tranimate">[docs]</a>    <span class="k">def</span> <span class="nf">tranimate</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="o">**</span><span class="n">kwargs</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Animate a 3D coordinate frame</span>
<span class="sd">    </span>
<span class="sd">        :param T: an SO(3) or SE(3) pose to be displayed as coordinate frame</span>
<span class="sd">        :type: numpy.ndarray, shape=(3,3) or (4,4)</span>
<span class="sd">        :param nframes: number of steps in the animation [defaault 100]</span>
<span class="sd">        :type nframes: int</span>
<span class="sd">        :param repeat: animate in endless loop [default False]</span>
<span class="sd">        :type repeat: bool</span>
<span class="sd">        :param interval: number of milliseconds between frames [default 50]</span>
<span class="sd">        :type interval: int</span>
<span class="sd">        :param movie: name of file to write MP4 movie into</span>
<span class="sd">        :type movie: str</span>
<span class="sd">        </span>
<span class="sd">        Animates a 3D coordinate frame moving from the world frame to a frame represented by the SO(3) or SE(3) matrix to the current axes.</span>
<span class="sd">    </span>
<span class="sd">        - If no current figure, one is created</span>
<span class="sd">        - If current figure, but no axes, a 3d Axes is created</span>
<span class="sd">        </span>
<span class="sd">    </span>
<span class="sd">        Examples:</span>
<span class="sd">    </span>
<span class="sd">             tranimate(transl(1,2,3)@trotx(1), frame=&#39;A&#39;, arrow=False, dims=[0, 5])</span>
<span class="sd">             tranimate(transl(1,2,3)@trotx(1), frame=&#39;A&#39;, arrow=False, dims=[0, 5], movie=&#39;spin.mp4&#39;)</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">anim</span> <span class="o">=</span> <span class="n">animate</span><span class="o">.</span><span class="n">Animate</span><span class="p">(</span><span class="o">**</span><span class="n">kwargs</span><span class="p">)</span>
        <span class="n">anim</span><span class="o">.</span><span class="n">trplot</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="o">**</span><span class="n">kwargs</span><span class="p">)</span>
        <span class="n">anim</span><span class="o">.</span><span class="n">run</span><span class="p">(</span><span class="o">**</span><span class="n">kwargs</span><span class="p">)</span></div>

<span class="k">if</span> <span class="vm">__name__</span> <span class="o">==</span> <span class="s1">&#39;__main__&#39;</span><span class="p">:</span>  <span class="c1"># pragma: no cover</span>
    <span class="kn">import</span> <span class="nn">pathlib</span>
    <span class="kn">import</span> <span class="nn">os.path</span>

    <span class="n">exec</span><span class="p">(</span><span class="nb">open</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">pathlib</span><span class="o">.</span><span class="n">Path</span><span class="p">(</span><span class="vm">__file__</span><span class="p">)</span><span class="o">.</span><span class="n">parent</span><span class="o">.</span><span class="n">absolute</span><span class="p">(),</span> <span class="s2">&quot;test_transforms.py&quot;</span><span class="p">))</span><span class="o">.</span><span class="n">read</span><span class="p">())</span>
</pre></div>

          </div>
          
        </div>
      </div>
      <div class="sphinxsidebar" role="navigation" aria-label="main navigation">
        <div class="sphinxsidebarwrapper">
<p class="logo">
  <a href="../../../index.html">
    <img class="logo" src="../../../_static/../../../../figs/icon.png" alt="Logo"/>
    
  </a>
</p>



<p class="blurb">Spatial maths and geometry for Python</p>




<p>
<iframe src="https://ghbtns.com/github-btn.html?user=petercorke&repo=spatialmath-python&type=watch&count=true&size=large&v=2"
  allowtransparency="true" frameborder="0" scrolling="0" width="200px" height="35px"></iframe>
</p>





<h3>Navigation</h3>
<ul>
<li class="toctree-l1"><a class="reference internal" href="../../../intro.html">Introduction</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../spatialmath.html">Classes and functions</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../indices.html">Indices</a></li>
</ul>

<div class="relations">
<h3>Related Topics</h3>
<ul>
  <li><a href="../../../index.html">Documentation overview</a><ul>
  <li><a href="../../index.html">Module code</a><ul>
  </ul></li>
  </ul></li>
</ul>
</div>
<div id="searchbox" style="display: none" role="search">
  <h3 id="searchlabel">Quick search</h3>
    <div class="searchformwrapper">
    <form class="search" action="../../../search.html" method="get">
      <input type="text" name="q" aria-labelledby="searchlabel" />
      <input type="submit" value="Go" />
    </form>
    </div>
</div>
<script>$('#searchbox').show(0);</script>








        </div>
      </div>
      <div class="clearer"></div>
    </div>
    <div class="footer">
      &copy;2020, Peter Corke.
      
      |
      Powered by <a href="http://sphinx-doc.org/">Sphinx 2.4.4</a>
      &amp; <a href="https://github.com/bitprophet/alabaster">Alabaster 0.7.12</a>
      
    </div>

    

    
  </body>
</html>